Kalman filter-1

from ： Kalman filtering （kalman filter） What does it have to do with mean filtering ？ - You know

Read an article 《 Kalman filter that a fool can understand （ Translated from Internet blog ）》, I found that many people in the comments said they couldn't understand , Decided to write a Kalman filter that a real fool can understand .

The article starts with a simple average , The problem goes from simple to complex , Finally, Kalman filter is introduced .

One 、 Average

for instance ： Here's the picture , There is a slider on a rod , There is a ranging laser head at one end of the rod , Now measure the position of the slider .

[ Laser head ]->_______[ slider ]__________

Three measurements ：1 rice 、0.9 rice 、1.2 rice

Obviously take the average (1+0.9+1.2)/3=1.033 Meters are more accurate .

Two 、 weighted average

Suppose there are two laser heads ：

[ Two laser heads ]=》________[ slider ]_________

Two laser heads simultaneously measure ：1.1 Rice and 1.5 rice

Average 1.3 Is rice more accurate ？ not always , If the measurement accuracy of two laser heads is the same , That average 1.3 Rice is more accurate .

If their measurement accuracy is different , Such as laser head 1 The measurement error variance is 0.01, Laser head 2 The error variance is 0.1, Then separate laser head 1 The measured value of 1.1 Mean meter ratio 1.3 Meters are more accurate , Because the laser head 2 It's too bad , You can calculate the variance of the average

  The variance of the average value is higher than that of the laser head 1 The error variance of is larger , Obviously, the average is not good . But the laser head 2 Is the measurement of completely useless ？ You can use another method to calculate the average , It is called weighted average

  Obviously with 0.9 and 0.1 Do the weighted average , The variance decreases , The weighted average is more accurate than both measurements . How much is the best weight of the two weighted average values ？ It can be assumed    A weight of a, be    A weight of (1-a), Take it in and calculate

  This is about a The quadratic function of ,  The variance of the weighted average is the minimum ( The extreme value problem of quadratic function in high school mathematics ), That is to say    The ratio of the weighting coefficients of is equal to the reciprocal of the ratio of their variances .

 

3、 ... and 、 Kalman filtering

Now the slider is not stationary , It's athletic , Suppose uniform motion ,1 rice / second , The laser head measures once per second

   Express n Time slider position ,   Express n The measured value of the laser head at the moment ,   Indicates the measurement error . Obviously, we can't average or weighted average the multiple measurements of the laser head now , Because the slider is moving . What shall I do? ？ The simplest way is to put    As a result , That is to say    The estimate of  , But this is not the best , Because the equation of motion of the slider is not used . Then use the simplest way to average

  Is this calculation better than directly using the measured value as the result , You can calculate the variance of their estimates

  Obviously, this method of averaging estimates smaller variance , When n As we go to infinity   . You can also change the initial value of the estimated value , For example 0 As initial value

By the same calculation, we can find ,n As we go to infinity , Variance of estimated value    still   , That is, as long as the number of iterations is enough , The error is independent of the initial value , All converge to the same value .

In fact, the above average value is not optimal , You can try to calculate the weighted average of each step , The ratio of the two weights is also the reciprocal of the ratio of variances . Again you will find , The variance of the estimate will converge , The convergence value is independent of the choice of initial value .

Did you find it here , The above result is very similar to Kalman filter , In fact, this is Kalman filter , But the state transition equation and measurement equation are particularly simple . Replace the equation of state and measurement equation with standard equation , Same as above , Each iteration uses the best weighting coefficient to calculate the weighted average , You will find that this is the standard Kalman filter , Just remember that the ratio of the best weighting coefficients is equal to the reciprocal of the ratio of variances . in other words , In fact, Kalman filter is to choose the weighted average of the best weighting coefficient .

The above equation of state and measurement equation are one-dimensional , What if they are all multidimensional ？ Multidimensional equations need to be solved by the least square method . The one-dimensional least squares solution is the mean , The multi-dimensional least square solution can be understood as the multi-dimensional average , And the weighted least square solution is the weighted average . So for the multidimensional equation of state and measurement equation , At each iteration, the weighted least square method is used to calculate the state estimation value .

Conclusion ： The essence of Kalman filter is to choose the weighted average of the best weighting coefficient ; The weighted least square method is to find the multi-dimensional weighted average .

What's the use of understanding this ？ I found that many people see problems related to filtering , Just want to toss the seemingly advanced Kalman filter , It's not necessary , If average or weighted average is used ( Multidimensional correspondence least square method or weighted least square method ) All measured values can be included , Generally, there is no need to toss Kalman filter , It's essentially the same , Unless Kalman filtering can introduce additional and more accurate observations .