Kalman filter_ Recursive Processing

【 Kalman filter 】1_ A recursive algorithm _Recursive Processing

The article is a personal summary based on the above video

  Get the formula

Current estimate = Last estimate + coefficient （ Current measured value - Last estimate ）

nature

K The bigger it is ,Xk Tend to be Xk-1, The smaller the impact of measurement results

1/K= coefficient =Kk      Estimation error e(est)    Measurement error e(mea)

Kk =  e(est)K / ( e(est)K-1 + e(mea)K )

nature

e(estK-1) >> e(meaK)   Kk Approach 1 Trust the current measurement

e(estK-1) << e(meaK)   Kk Approach 0 Believe the last measurement

There are three important formulas

Step2:Kk =  e(est)K / ( e(est)K-1 + e(mea)K )

Step2:Xk = Xk-1 + Kk * (Zk-Xk-1)

Step3:e(estK) = (1-Kk) * e(estK-1)

Use the example of measuring the width of coins

Actual length X=50mm  Estimated value    X0=40mm  Estimation error e(est0) = 5mm Measured value Z1=51mm Measurement error eMea=3mm

The first measured value is 51mm The second measurement is 48mm 

Calculate the value of the second line from the above formula

Kk=5/(5+3)=0.625   Xk= 40 + 5/8(51-40) = 46.875  e(est)=(1-5/8)*5=1.875

The following analogy can be used to get such values and images （ Blue Zk, Red Xk） 

result ：

Xk Estimates are getting closer 50

Zk The measured values are distributed in 50 about , The error shall not exceed plus or minus 3

A simple Kalman filter is implemented , A recursive algorithm