Make writing a habit together! This is the sixth day of my participation in the “Gold Digging Day New Plan · April More text Challenge”. Click here for more details.

🍊 About the author: Bald Sue, dedicated to describing problems in the most colloquial language

🍊 Previous review: Binocular stereo matching steps cost polymerization of binocular stereo matching

🍊 Support for SiU: like 👍🏼, save ⭐, leave a message 👍

    

🥂 original is not easy, reprint please attach the original link 🥂

🥗 Kalman filter series 1 — Kalman filter

🍦 comes first

Finally start to write the kalman filter series, how about a little excited 🍭 🍭 🍭 actually in previous posts I also simply introduced kalman filtering, enmmm 🧅 🧅 🧅 yes, simply introduced, main is to help people intuitively understand kalman filter, and not to explain and kalman filtering formula derivation, so this series? I will focus on the derivation of some formulas to kalman filter with our chatter 🍒🍒🍒

Formula derivation you mentioned the possibility of a little resistance, the articles I wrote before I have also found that practical articles seem to always is much higher than the number of theoretical articles to read, but theory also is essential to us, we should be calm face 🌼 🌼 🌼 this article, I’m going to introduce some theoretical problems, but also brought by some examples, Kalman’s core derivation and I’ll give you a link to the video of the derivation, which is probably more effective than a written description. This means that there is not much derivation in this article, but it introduces some important concepts. If you are interested in the derivation, you can watch the video 🍳🍳🍳.

 

🍦 Kalman filtering macro understanding

First of all, let’s do a macro understanding of Kalman filter, that is, what can we do with Kalman filter? Kalman filter is used to estimate the state of things, why do we estimate the state of things? It must be that we cannot accurately know the current state of things, so we have to make an estimate at this time [but we cannot make an estimate casually, and we usually make an estimate based on some experience, so the estimate result is relatively accurate]. On the other hand, in order to estimate the state of a thing, we can also use some instruments to measure, but there will be some measurement errors with instruments. And that raises the question, should we trust our estimates or our measurements? I think there’s a good chance that people think, well, if we’re using a shoddy measuring instrument that we bought for 50 cents from a small manufacturer, then we’re more likely to trust our estimates. If the device is worth a million dollars, we have reason to trust the results. In fact, we tend to assign different proportions to these two outcomes, indicating which one we trust more. Then Kalman filter is a method to estimate the state of things by combining the two methods of prior estimation and instrumental measurement.

Let’s take an example to illustrate the above meaning. As shown in the picture below, we used the stove to boil water for the kettle, now the water is boiling, we estimate the temperature of the water is 95.4°C [affected by atmospheric pressure, there is no 100 degrees 🎈], that is, Xk=95.4°C. At this point we used a thermometer to measure the temperature of the water and found that it was yK =94.1°C. Who to believe when XK and Yk disagree? We assume that the confidence of the thermometer is 0.6, and the estimated temperature is 0.4, so the final water temperature =0.6*94.1+0.4*95.4=94.62.

 

---

---

🍦 Kalman filter boot small example

From the above you may have a general understanding of kalman filter, that is, it is used to estimate the state of some uncertain objects. Here is a small example in daily life to introduce Kalman filter: there is a coin and a ruler, and the purpose is to get the size of the coin as accurately as possible. It is measured k times in total, and the measurement result of each time is marked as Z1, Z2… Zk. How can we estimate the size of a coin? A natural idea is to take an average, as shown below:

What point do I want to make with this example? We can focus on the final result of the previous example: X – 1 + 1 = X ^ ^ k k k (Zk – X ^ k – 1) {{\ rm {\ hat X}} _k} {\ rm = {}} {{\ rm {\ hat X}} _ {k – 1}} + \ frac {1} {k} ({Z_k} – {{\ rm {\ hat X}} _ {k – 1}}) X ^ ^ k – 1 + k = X k1 (Zk – X ^ k – 1). It can be found that the estimated value after k measurements = the estimated value after (K-1) measurements + 1k\frac{1}{k}k1 (measurement result for the k- estimated value after (K-1) measurements). In fact, Kalman filter contains such recursion idea 🍉🍉🍉. If 1k\frac{1}{k}k1 approaches 0 as the number of measurement k increases, the formula above will be: X = X ^ ^ k k – 1 {{\ rm {\ hat X}} _k} {\ rm = {}} {{\ rm {\ hat X}} _ {k – 1}} X = X ^ ^ k k – 1, namely with the increase of k, the first k (k – 1) estimates only, and the associated time estimates, and has nothing to do with the first k times measured value, So as k goes up, it doesn’t matter. And this makes a lot of sense, because when we measure it many times, the estimate is actually very close to the truth, so it doesn’t make much sense to measure it one more time; On the contrary, if k is very small, that is, k=1 (k represents the number of measurements and k=0 is meaningless), the above equation will be: X^k=Zk{{{\rm{\hat X}}} _K}{\rm{=}}{Z_k}X^k=Zk, that is, the estimated value at this time is entirely determined by the measured value. In fact, the above equation is similar to a formula in Kalman filter, of course, this is a later story, we can pay attention to here. Further, we can replace 1k\frac{1}{k}k1 with Kk, which is the Kalman coefficient.

Here I also want to give the calculation formula of Kk, as shown in the figure below:

Of course, there is no need to understand why, just have a general impression. We understand this formula at a macro level:

• when
  ${e_{EST}} \gg {e_{MEA}}$, that is, when the estimation error is much greater than the measurement error, K_k1,
  ${{{\rm{\hat X}}}_k}$Tend to
  ${\rm}{Z_k}$, indicates that you trust the measured value.
• when
  ${e_{EST}} \ll {e_{MEA}}$, that is, when the estimation error is far less than the measurement error, K_kTends to zero,
  ${{{\rm{\hat X}}}_k}$Tend to
  ${{\rm{\hat X}}_{k – 1}}$, indicating that you believe the estimate.

 

🍦 Kalman filter data fusion example

Since you have seen kalman filter, you must know that Kalman filter has the role of data fusion. In fact, Kalman filter plays a role of data fusion. It can be seen from the writing of this article in front, Kalman filter is not the estimated value and measured value of the fusion and get a new result 🍓🍓🍓 read here do not know readers have doubts, we have not introduced the process of Kalman filter, how to directly jump to the application of Kalman filter. This is true, but WHAT I want to do is move on to the derivation of Kalman’s formula with some examples, which are of course very simple examples, so let’s take a look at this example 🥝🥝🥝

An electronic scale and an ordinary scale can measure an object at the same time. The weight obtained by the electronic scale is Z1=30, and the standard deviation is σ1=2{\sigma _1} =2 σ1=2. The weight measured by ordinary scales is Z2=32, and the standard deviation is σ1=4{\sigma _1} =4 σ1=4. We hope to obtain an optimal estimate 🍐🍐🍐 by using these two results

First of all, we can draw the distribution diagram of the weight of the object measured by the two scales, as follows: [Assuming they meet the normal distribution]

In the figure above, Z1 represents the distribution of measurement results of electronic scale, and Z2 represents the distribution of measurement results of ordinary scale. Before we do the math, I think we can make a guess. It can be seen from the above that the standard deviation of the electronic scale is only 2, while the standard deviation of the ordinary scale is 4, which means that the measurement result of the electronic scale is more stable, so the measurement result may be more accurate, and the final result of our fusion may be more inclined to that of the electronic scale, that is, closer to 30. Of course, these are just my guesses, so let’s do some calculations to see if the best estimate matches our guesses.

Let’s first clarify our goal: to estimate the true value from two measurements. Directly speaking, in the small example guided by Kalman filter, we have also made an estimate of the real size of the coin. Can we use it for reference? In the lead example, we ended up with a formula for the coin estimate: X – 1 + 1 = X ^ ^ k k k (Zk – X ^ k – 1) {{\ rm {\ hat X}} _k} {\ rm = {}} {{\ rm {\ hat X}} _ {k – 1}} + \ frac {1} {k} ({Z_k} – {{\ rm {\ hat X}} _ {k – 1}}) X ^ ^ k – 1 + k = X k1 (Zk – X ^ k – 1). Current estimate = last estimate + coefficient *(current measurement – last estimate). So can we use this to our advantage? Some people might say there are no estimates in this example, that means your mind is locked, why can’t we just take one of our measurements as an estimate 🍁🍁🍁 now we take the first measurement Z1 as an estimate, Z2 as a measure, Thus we can obtain the expression for the estimated true value Z^{{{\rm{\hat Z}}}}Z^ as follows:

Z ^ = Z1 + k (Z2 – Z1) \ hat Z = {Z_1} + k ({Z_2} {Z_1} Z ^ = Z1 + k (Z2 – Z1)

What can we do if we get this formula, and we have an unknown coefficient k, so our first step is definitely to solve for this k. Our goal is the optimal results of measuring Z ^ {{{\ rm {\ hat Z}}}} Z ^, if we make the Z ^ {{{\ rm {\ hat Z}}}} Z the sigma Z ^ ^ {\ sigma _ {\ hat Z}} sigma Z ^ the smallest, I.e. variance var (Z ^) {\ mathop {\ rm var}} \ hat (Z) minimum var (Z ^), can make the Z ^ {{{\ rm {\ hat Z}}}} Z ^ optimal results. Var (Z^){\mathop{\rm var}} (\hat Z)var(Z^) {\mathop{\rm var}} (\hat Z)var(Z^)

Above, we have got the sigma Z ^ 2 = 12 + k2 (1 – k) 2 sigma sigma 22 {\ sigma _ {\ hat Z}} ^ 2 = {{\ rm {(1 – k)}} ^ 2} {\ sigma _1} ^ 2 + {k ^ 2} {\ sigma σZ^2{\sigma _{\hat Z}}^2σZ^2 =(1−k)2σ12+k2σ22, σZ^2{\sigma _{\hat Z}}^2σZ^2 =(1−k)2σ12+k2σ22

So far we have obtained the final estimated measurement value and standard deviation. It can be seen that the result is closer to 30 as we guessed before, and the standard deviation of this result is smaller than the standard deviation of the previous two measurements. After_merge = after_merge = after_merge = after_merge = after_merge = after_merge = after_merge

 

🍦 Kalman filter formula

This section will introduce you to Kalman’s formula 🍺🍺🍺 again through a small example. As shown in the figure below, there is a car running on the road, its speed is S, its acceleration is A, and its position at the current moment is represented by P.

According to the knowledge of uniformly accelerated linear motion in junior high school physics, the formula can be obtained:

{Pk = Pk – 1 + 1 Δ Sk – t + 12 a ⋅ Δ t2Sk = Sk – 1 + a ⋅ Δ t \ left \ {\ begin {array} {l} {P_k} = {P_ {k – 1}} + {S_ {k – 1}} \ Delta t + \ frac {1} {2} a \ cdot \ Delta t ^ 2} {\ \ {S_k} = {S_ {k – 1}} + a \ cdot \ Delta t {array} \ \ end right. {Pk = Pk – t + 1 + 1 Δ Sk – 21 a ⋅ Δ t2Sk = Sk – 1 + a ⋅ Δ t

Where Pk represents the position of the car at time K, PK-1 represents the position of the car at time k-1, Sk represents the speed of the car at time K, SK-1 represents the speed of the car at time K-1, and δ T \Delta t δ t represents time interval. For the above equation we can write it in vector form as follows:

$\left[ \begin{array}{l} {P_k}\\ {S_k} \end{array} \right] =\left[\begin{array}{cc} 1 & {\Delta t} \\ 0 & 1 \end{array}\right]\left[ \begin{array}{l} {P_{k – 1}}\\ {S_{k – 1}} \end{array} \right] + \left[ \begin{array}{l} \frac{1}{2}\Delta {t^2}\\ \Delta t \end{array} \right] \cdot a$

We make the Xk = {X_k} = \ [PkSk] left [\ begin {array} {l} {P_k} \ \ {S_k} {array} \ \ end right] Xk = [PkSk]. The Xk – 1 = [Pk sk – 1-1] {X_ {k – 1}} = \ left [\ begin {array} {l} {P_ {k – 1}} \ \ {S_ {k – 1}} {array} \ \ end right] Xk – 1 = [Pk sk – 1-1]. Make A = A = \ [1 Δ t01] left [\ begin {array} {cc} 1 & {t} \ Delta \ \ 0 & 1 {array} \ \ end right] = Δ t1 [10], B = [12] Δ t2 Δ t = B \ left [ \ begin {array} {l} \ frac {1} {2} \ Delta t ^ {2} {array} \ \ \ \ Delta t end \ B = right] [21] Δ t2 Δ t, Uk – 1 = a {U_ {k – 1}} = = aUk – 1 a. Xk is called the state vector, A is called the state transition matrix, B is called the control matrix, and UK-1 is called the state control vector. Note that the motion here is uniformly accelerated, i.e., a does not change, then Uk=Uk-1=a. Then the above formula can be changed into the following form:

Xk = ⋅ ⋅ Xk – 1 + B Uk – 1 \ {X_k} = A cdot {X_ {k – 1}} + B \ cdot {U_ {k – 1}} Xk = ⋅ ⋅ Xk – 1 + B Uk – 1

---

Attention, we are all the above mentioned formula is established in the ideal state, but we know that the ideal state in reality it is not possible, so we need to add noise, in the above formula, after transformation formula is as follows: (note that this noise is meet the needs of normal distribution 】

Here we repeat the formula with noise: Xk = ⋅ ⋅ Xk – 1 + B Uk – 1 + Wk – A 1 \ {X_k} = A cdot {X_ {k – 1}} + B \ cdot {U_ {k – 1}} + {W_ {k – 1}} Xk = ⋅ ⋅ Xk – 1 + B Uk – 1 + Wk – 1 A. The above equation is an unknown quantity with noise, so we cannot model it. Therefore, we cannot add WK-1 into our estimation model, but the model must be inaccurate after removing wK-1, so we use X^k{{\hat X} _K}X^k to represent the original Xk{X_k}Xk, and add ^ to represent the estimated value. Thus, the model we established is as follows:

So we have a mathematical model for our estimation: – = X ^ k ⋅ ⋅ X ^ k – 1 + B Uk – {{\ hat X} _k} ^ 1 – = A \ cdot {{\ hat X} _ {k – 1}} + B \ cdot {U_ {k – 1}} – X ^ k = ⋅ ⋅ X ^ k – 1 + B Uk – 1 🥂 🥂 🥂

---

---

Our estimation model has been obtained above, which is the same as the previous car. Now there is a small satellite in the sky that can measure the speed of the car, as shown in the picture below:

Because the satellite can directly obtain the position and speed of the car, there are the following equations:

​
$\left\{ \begin{array}{l} {Z_{p,k}} = {P_k}\\ {Z_{s,k}} = {S_k} \end{array} \right.$

Rewrite it in vector form as follows:

​
$\left[ \begin{array}{l} {Z_{p,k}}\\ {Z_{s,k}} \end{array} \right] = \left[ {\begin{array}{cc} 1&0\\ 0&1 \end{array}} \right]\left[ \begin{array}{l} {P_k}\\ {S_k} \end{array} \right]$

The Zk = [Zp, kZs, k] {Z_k} = \ left [\ begin {array} {l} {Z_ {p, k}} \ \ {Z_ {s, k}} {array} \ \ end right] Zk =] [Zp, kZs, k, H = [1001] H = \ left [{\ begin {array} {cc} \ \ & 0 0 1 &1 \ end {array}} \ right] H = [1001], Xk = {X_k} = \ [PkSk] left [\ begin {array} {l} {P_k} \ \ {S_k} {array} \ \ end Xk = right] [PkSk]. ⋅Xk{Z_k} =H \cdot {X_k}Zk=H⋅Xk🌱🌱🌱

Similarly, there are also errors in the measurement, which is also considered to meet the normal distribution. Add errors into the above formula, and the results are as follows:

⋅Xk+Vk{Z_k} =H \cdot {X_k} + {V_k}Zk=H⋅Xk+Vk🌴🌴🌴 Also because of the unknown number Vk, we cannot model it. The modeling results of observation equation without noise are as follows:

So we get the we measure, the mathematical model of the: X ^ k = H – 1 zk {{\ hat X} _k} = {H ^ {1}} {Z_k} X ^ k = H – 1 zk 🥂 🥂 🥂

---

At this point, we have obtained an estimation model, an observation model. At this time, we can use the conclusions in the guidance and fusion small cases above to fuse the estimated value and the observed value, and the results are as follows:

We can make a simple analysis of the above equation:

• When Kk = 0 X = X ^ ^ k k – {{\ hat X} _k} = {{\ hat X} _k} – ^ ^ – ^ k = X X k, namely a posteriori estimates = a prior estimate.
• When Kk = H – 1, X ^ k = H – 1 zk {{\ hat X} _k} = {H ^ {1}} {Z_k} X ^ k = H – 1 zk, namely a posteriori estimates = measurements.

Got X = X ^ ^ k k – + Kk (Zk – HX – ^ k) = {{\ hat X} _k} {} {\ hat X _k} ^ – + {K_k} ({Z_k} – {{\ hat X} _k} H ^ -) – X = X ^ ^ k k + Kk (Zk – HX – ^ k) this formula, Kk is unknown, we can use the method in fusion small case to find Kk, that is, to find the variance of X^k{{\hat X}_k}X^k minimum Kk, then Kk is the solution. I’m not going to do the derivation here, but if you’re interested, you can watch this video. Finally we get the expression for Kk: Kk = Pt – HT (HPt – HT + R) – 1 {K_k} = {P_t} ^ – ^ T {H} {{P_t} (H ^ – ^ T} {H + R) ^ {1}} Kk = Pt – HT (HPt – HT + R) – 1 🍁 🍁 🍁 on one type of Pt – ^ – {P_t} Pt – for time t process noise covariance of transcendental value, it is equal to: Pt – = APt – 1 at + Q {P_t} ^ – = A {P_ {t – 1}} {A} ^ t + QPt – = APt – 1 at + Q 🍀 🍀 🍀 of Q is the measurement noise covariance. Pt – {P_t} ^ – Pt – as the empirical value, the subsequent need to update, updated for Pt and Pt = (1 – KkH) Pt – {P_t}, {P_t} = (1 – H) {K_k} {P_t} ^ – Pt and Pt = (1 – KkH) Pt – 🌴 🌴 🌴

The most important part of Kalman filter is the five formulas, two predictions and three updates, which I have already mentioned in the previous article. Now I do some sorting, as shown in the figure below:

At this point, the Kalman formula is introduced, I hope you have a harvest 🌾🌾🌾

🍦 Application examples of Kalman formula

In fact, there are a lot of materials on the Internet for this part. I have made a small example in my previous article. I haven’t decided what additional examples to add

🍦 epilogue

This paper did spend a lot of time, with too many formulas and other things, but it also gained a lot. I hope it can give you a clearer understanding of Kalman filter. Of course, there are some expressions in the paper that are not particularly clear.

The initial plan is to update the extended Kalman filter and the boundless Kalman filter in the future, stormed 🌻🌻🌻

🍦 Reference link

DR_CAN:www.bilibili.com/video/BV1hC…

421 shi team: www.bilibili.com/video/BV1Rh…

\