diff --git a/.obsidian/plugins/text-extractor/cache/da84fc3578b9774a320e54b11ad4d1ed.json b/.obsidian/plugins/text-extractor/cache/da84fc3578b9774a320e54b11ad4d1ed.json new file mode 100644 index 0000000..0e7db93 --- /dev/null +++ b/.obsidian/plugins/text-extractor/cache/da84fc3578b9774a320e54b11ad4d1ed.json @@ -0,0 +1 @@ +{"path":"Attachments/Pasted image 20250612221908.png","text":"| 0. Set initial values: z By | . Predict state & error covariance: - êk— -Aik—l A ) P = AR A\" +Q pricos Z Il . Compute Kalman gain: H. T À — ) K, =P H\" (HP H\" +R) . s - lll. Compute the estimate: 1 D % =% +K,(7, — HR ) |","libVersion":"0.3.2","langs":"deu+eng+fra"} \ No newline at end of file diff --git a/.obsidian/workspace.json b/.obsidian/workspace.json index 5d78cc5..90e8dd1 100644 --- a/.obsidian/workspace.json +++ b/.obsidian/workspace.json @@ -50,18 +50,6 @@ "icon": "lucide-file", "title": "Kalman Filter" } - }, - { - "id": "da60fe5e3074c644", - "type": "leaf", - "state": { - "type": "excalidraw", - "state": { - "file": "99 Work/0 OneSec/OneSecNotes/30 Engineering Skills/Robotics/kalman_filter.excalidraw.md" - }, - "icon": "excalidraw-icon", - "title": "kalman_filter.excalidraw" - } } ], "currentTab": 2 @@ -121,7 +109,7 @@ } ], "direction": "horizontal", - "width": 400.50390243530273 + "width": 267.50390243530273 }, "right": { "id": "1017d1cf4a473028", @@ -311,8 +299,9 @@ "periodic-notes:Open today": false } }, - "active": "84f0fd68d3885e08", + "active": "b2f0aff0beb8228f", "lastOpenFiles": [ + "Attachments/Pasted image 20250612221908.png", "99 Work/0 OneSec/OneSecNotes/30 Engineering Skills/Robotics/kalman_filter.excalidraw.md", "99 Work/0 OneSec/OneSecNotes/30 Engineering Skills/Robotics/Kalman Filter.md", "OneNote/PhD/Things to know very well when applying for a robotics PhD.md", @@ -354,7 +343,6 @@ "2 Personal/Lists/Packlisten/Packliste - Berge Mehrtages.md", "2 Personal/Hobbies/Berge/Gletscherkurs/Gletscherkurs - Selbstudie.md", "2 Personal/Home Lab/Repairshop/Repair Climbing Light.md", - "99 Work/0 OneSec/OneSecNotes/10 Projects/Requirements/Requirements Gathering.md", "Attachments/Pasted image 20250402091926.png", "Attachments/Pasted image 20250331151348.png", "Attachments/IMG_5894.jpeg", diff --git a/99 Work/0 OneSec/OneSecNotes/30 Engineering Skills/Robotics/Kalman Filter.md b/99 Work/0 OneSec/OneSecNotes/30 Engineering Skills/Robotics/Kalman Filter.md index f1ed258..711b812 100644 --- a/99 Work/0 OneSec/OneSecNotes/30 Engineering Skills/Robotics/Kalman Filter.md +++ b/99 Work/0 OneSec/OneSecNotes/30 Engineering Skills/Robotics/Kalman Filter.md @@ -9,10 +9,87 @@ tags: The kalman filter is like the alpha filter (e.g. low pass filter) but with a dynamically adjusted alpha. Meaning it adapts the corner frequency dynamically when filtering things. -```mermaid -graph TD -a --> b -``` +We have measurements of some kind $z_k$, which are fed to the kalman filter and then it provides estimates of another thing $\hat{x}_k$. The measurements and the estimates don't have to be the same quantity. We could have sonar data (altitude measurements) and estimate velocity with them, because we know the relation (e.g. we have a model) between them. +Within the kalman filter there is a prediction state and an estimation step. + +1. Predict state +the minus sign (-) indicates a prediction +The prediction happens with a simplified linear model which uses the previous estimate as an input: +$$ \hat{x}^-_k = A \hat{x}_{k-1}$$ +Then we have the error covariance (predicted), which is a measure of noise and it influences how $\alpha$ is adapted (in our lowpass filter analogy of above). +$$ P_k^- = AP_{k-1} A^T + Q$$ +2. Estimation Step: + Compute the Kalman Gain: which is related to $\alpha$, meaning it can be interpreted a bit like a dynamic gain, which depends on the error covariance $P_k^-$, + $$ K_k = P^-_kH^T(HP^-_kH^T+R)^{-1} $$ + And then you compute the estimates: + $$ \hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}_k^-) $$ + +3. Compute the error covariance + $$ P_k = P_k^- - K_k H P_k^- $$ + +| External Input | $z_k$ (measurement) | +| ------------------------ | ------------------------------ | +| Final Output | $\hat{x}_k$ (estimate) | +| System Model | $A, H, Q, R$ | +| For internal computation | $\hat{x}_k^-, P_k^-, P_k, K_k$ | +- $A$: Best estimate of a linear model of the process going on +- $H$: How do the measurements relate to the state (estimate) +- $Q, R$: related to noise levels (diagonal matrices) --> tune those. + +### Overview +- $\hat{x}_k \in \mathbb{R}^{n \times 1}$ at time $t_k$ +- $x_k \in \mathbb{R}^{n \times 1}$ : State variable +- $z_k \in \mathbb{R}^{m \times 1}$: Measurement +- $A \in \mathbb{R}^{n \times n}$ : State transition matrix +- $H \in \mathbb{R}^{m \times n}$: state to measurement matrix +- $w_k\in \mathbb{R}^{n \times 1}$ : state transition noise () +- $v_k\in \mathbb{R}^{m \times 1}$ : measurement noise (usually information in datasheet of sensors) +- $Q \in \mathbb{R}^{n \times n}$ : covariance matrix of $w_k$ --> state transition noise / process noise +- $R \in \mathbb{R}^{m \times m}$ : covariance matrix of $v_k$ --> measurement noise + +This is based on two linear state models models: +- $x_{k+1} = Ax_k + w_k$: state model (similar to control theory) +- $z_{k+1}=Hx_k + v_k$: sensor model + +Q and R are tuned from trial and error. R can sometimes be tuned from the sensor datasheet. + +![[Pasted image 20250612221908.png]] +### Prediction Step +We're trying to estimate a state $x_k$ ($k$ is just a time step) which we call $\hat{x}$. +We're assuming that the true state is modeled with a normal distribution where the mean is our estimate $\hat{x}_k$ and the variance is our error covariance matrix $P_k$ which is just noise essentially: +$$ x_k \sim \mathcal{N}(\hat{x}_k, P_k)$$ + +#### I. Prediction +- $A, Q$ just show up here: +- Q: is the process noise: it's just there you cannot really get rid of it +$$ \hat{x}^-_k = A \hat{x}_{k-1}$$ +$$ P_k^- = AP_{k-1} A^T + Q$$ +### Estimation Step +#### II. Compute Kalman Gain +This is the dynamic update of the Kalman gain. + $$ K_k = P^-_kH^T(HP^-_kH^T+R)^{-1} $$ +#### III. Compute the estimate +- $H\hat{x}_k^-$: This is just the predicted measurement, because H relates x to z +- $K_k$: how much do we trust the measurement - the predicted measurement to update our estimate? +$$ \hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}_k^-) = (\mathbb{I}-K_k)\hat{x}_k^- + K_k z_k$$ + If you look at the equation above in detail you can see that it looks very much like an alpha filter. If we assume that $\alpha = (1-K_k)$ we can see the equation $\hat{x}_k^- = \alpha \hat{x}_k^- + (1-\alpha)z_k$, which looks exactly like the alpha filter (low-pass filter). Only that the Kalman gain is updated based on measurement noise and confidence values. + +#### IV. Compute the error Covariance +The Error covariance gets updated dynamically as well. + $$ P_k = P_k^- - K_k H P_k^- $$ + +Error covariance is a measure of the *inaccuracy* of the estimate. The larger the covariance the more inaccurate is the estimate. + +Note: +- $H, R$ only show up in the esimation step. +- $H$: is the measurement noise: you can control it with better sensors + + +### Simple Example + +--- ## Sources -- [Kalman Filter for Beginners, Part 1 - Recursive Filters & MATLAB Examples - YouTube](https://www.youtube.com/watch?v=HCd-leV8OkU) \ No newline at end of file +- [Kalman Filter for Beginners, Part 1 - Recursive Filters & MATLAB Examples - YouTube](https://www.youtube.com/watch?v=HCd-leV8OkU) + +$E=2$ diff --git a/99 Work/0 OneSec/OneSecNotes/30 Engineering Skills/Robotics/kalman_filter.excalidraw.md b/99 Work/0 OneSec/OneSecNotes/30 Engineering Skills/Robotics/kalman_filter.excalidraw.md deleted file mode 100644 index 8f7964d..0000000 --- a/99 Work/0 OneSec/OneSecNotes/30 Engineering Skills/Robotics/kalman_filter.excalidraw.md +++ /dev/null @@ -1,44 +0,0 @@ ---- - -excalidraw-plugin: parsed -tags: [excalidraw] - ---- -==⚠ Switch to EXCALIDRAW VIEW in the MORE OPTIONS menu of this document. ⚠== You can decompress Drawing data with the command palette: 'Decompress current Excalidraw file'. For more info check in plugin settings under 'Saving' - - -# Excalidraw Data -## Text Elements -0. Set initial Values - ^iV7ZqvaH - -%% -## Drawing -```compressed-json -N4KAkARALgngDgUwgLgAQQQDwMYEMA2AlgCYBOuA7hADTgQBuCpAzoQPYB2KqATLZMzYBXUtiRoIACyhQ4zZAHoFAc0JRJQgEYA6bGwC2CgF7N6hbEcK4OCtptbErHALRY8RMpWdx8Q1TdIEfARcZgRmBShcZQUebQBWbQAGGjoghH0EDihmbgBtcDBQMBKIEm4ygDUAdgAtAEd6XAAJVJLIWEQKqCwoNtLMbmcAFm0ATgBmAEZ4qYAOePjhgDY5 - -6vj+UpghnjGxhNX4sfi5ueWeCaTh6s3IChJ1bnm57XWxtaWp5fX41dupBCEZTSbjxJL/azKYLccGFARQUhsADWCAAwmx8GxSBUAMRTBD4/H9SCaXDYJHKRFCDjEdGY7ESBHWZhwXCBbLEiAAM0I+HwAGVYNCJIIPJzmAjkQgAOoPSTcPhwiASxEowUwYXoUXlf5U4EccK5NBTf5sVnYNTbY1JWHtCCU4RwACSxCNqDyAF1/lzyJkXdwOEI+f9CDS - -sBVcFNOVSaQbmG7A8GlWEEMRQfFqnMeElzmN/owWOwuGhpvmmKxOAA5ThiJ7THg8apJRYbJWEZgAEXSPTTaC5BDC/00whpAFFgplsm6Cu0inDSuUJAAlWoADQAivUAFYAFWI9CgAH1qpl4gBVfSR4a1YmlTridBsxFUecAXzhXqVQjgxFwPae1TDGMgEXMsizzP8RAcEiAZBvgkFsOSqbcP2+CDkqPSYH0EhJNoqD8ggUCoKGahWPgqCVAQQjhAA - -OlwuqUDuvQVLh+GEcRHCkQQFFUbR9FKlynBQPyhBGA+2bekJABiuD6LyVqoK2dqYVAACCRDKMW6DBFyfRlqQUDmAQ6lAlp0BmpyejZLgoZMP6aCJvBSpYkCoYEExWEsXhBFESRhncZRvh8ZyuBCFAbBLuEokPgi1GQbZzSAsC2GoFM2g8PEhSvpsxQLr26Arhu257gex6nheV43v894RqQz6coMaCnOlFx7GMSQXMMEzrP8CnODwwxpRlUxJGMI1 - -JBMg3VMB/z3MQjxoHEmYLFM0xzBMRzdUppSSElILGss+zrd8Y17Dw8w9UqkKarapQqlKdJYrihIEkgQ5khSMa0hiT2MuQHAsmyWR6QJvICkKD7KhiOrJpKKKyvN8qLf891qhDFTammurCPqhpPKa5qWk8Nr/A634utOn52j6skIPZqCOSGYaNY+PDRiOxBxm6uV3vA4lwtlyYIMhi2nCNczvCaSoFhWWmnPpsvVhwtbGvEEyrMMJzrSGnbdiLqCo - -ehdrDtSxDjhkwMJnB/zfr+/7GoBwHDKB4FzPF0GwUmdqYkh+WGwg/xwGwoY5Pk84zrOt3tEk85U+0EftEtpyzGtG2TJds5gF8R3q9NUxnRdmWzh6H6QaEUDovockyKmAAKwccmgCdgEnK2p5tGftNn2jHXnBcfKXsNslAABCoaOBwyjcDzkDpJOUD01UdSNC0t7cohQhurhSTzKscwjd1EynBMG1u/O9q4HA3AvNM6xXKcPD7xltyQDymB1w3KWO - -QL/xZMQY80lDFPBy1sh4GVUnVNgFBdq4HyozJUf8IHPhgflCAT4oGciCMOCg+t/ZZRyhhPmFRCCXmUG9aW5YizcCbArIsSsVaoBPtNVYk0pZ2nHuGCQuAJicnbF2YI9tUCxQDm2VBmgKAAHEACyEieCqS5JyHkfJ1SaikGSDQgRxRwxlHKfGsNVQIBUZDTG7M/CSC5nou0ZoyRE2tFHNBk8br/BZiNZIjYJoLBAoXXqQxlgTHGEcaoXxmEfBRtox - -6DJ0A4htDElI71yRkxpBE7o/1AbshBnaOaC1UCAX+LtIE+1UrLBRsLfKJ8cz2LJs6V0+Q46v19HTOBoC7RfQsSAr2pQTZjgnJbWpNsfx/n1lMR2IFNYZWGPYqCMF2lOW9ohFEfsBwiLtBKP8m8KiIEAZPRRvJGkVGWH42BmhfzxGwH4vAmgEDq2WEkXA8QuRnK+KsXA/ZzhTGIDvMYyxxTuAfM3NhXcPz4MKDzMoqCpjrgmFJWuyxVJT2qkQxkzF - -nHcAmHEMasxlhTFWo/KYz8lR9UWK8c6Y0sXvGqJca4s1dFNTyXtFK+K7TXQfPY1GaIfqRIgHiV6RJ4mfQ5skv6zJWTpJ2co9GIpoZY30VKBG2TFQrO0UYjGUrTG43jJY0o1iLSwGJpUqk1TKbegafTeB7DmYRmGKY2MeMZklP1hcaoQSZijVoZwVFbqOD0IfGMYYYsnVAR1vwwi+thFDg5ubeeVsOmQFtoM/KwygKjNmEkXJSopme1maUH2CyUJL - -OqsxCQ0ZGKFvQIooSIkxIKnsYJbIMk5L4AUttDovQTKaQqDpDJpQCyGXcG2sy4Ur7/CslEWypBTXNK1aQVyHB3KlogCFMKEUopVrQGG9NCV6VPHSkXMA74wBUzQXAOAgpBnT0KNAXamQO0FK7QwQgCAKAjw+ok769JcRck/V+/oEBsAiHSU6Ho+hBQGMFVEl6vKL1/rqsDQDGRn0JK+mB6AqSRXAx/dBgDQGpJg2VZKsUmxf3/tg0BkDsqaW8EI5 - -hkjGQyNow1MY1VVHiPZDg/oJcONzG2tSsxmDrGgMAHlCa6rsbxrDGQpLSVkvJUEYmaP6Ek9kSt4lbpEb4wvIDHk1IaTMp2jDLGNO0aiOAyB0CQhNI6Wp8T+hRw0iQVAlBtV6pyf4xkezFAdyIvQF9H9zBsCIj5KubgKxqgJAlr8dqY1mF5gvX5gL+AACawXAI7szHi7aEAjBsAMOeu0TQgpPGBaUajrn2MczaVqSQbIh0XspCQZT1bCN1eIIKBAV - -80AZea1ItgxAEC2dwJoYIiy0LLMgM1sDoKR4YlQaQZQpIAAU50bi8GGdQFby3cLxAAJSckisoIMbIKizYWxccEvBLhrdO2tzbO2iuQBK8JbRQnDKcGjVm7kDTIphmndstAoKsgDaG9wdddpsBEHa0I0gcUlSzuvWuqHo3IDCCgFBGKCO7sQDsFuBA2Acj8lnXAbrvX+uDdwfmi9ZJDKMB3Nl/AuXeZdBFOkXHVDh2b3CvoTzjOGaTsgDmsnI2y4S - -lUiz6ntPM1ZXAILV+uzubvlfEAA= -``` -%% \ No newline at end of file diff --git a/Attachments/Pasted image 20250612221908.png b/Attachments/Pasted image 20250612221908.png new file mode 100644 index 0000000..3ed645d Binary files /dev/null and b/Attachments/Pasted image 20250612221908.png differ