[Kalman filter] recursive algorithm

  Hello everyone, I am Xiaozheng. Recently, I am learning Kalman filter. This article records the recursive algorithm in the Kalman filter I learned . Through examples, let everyone understand more clearly what recursion is. I hope to have some exchanges with students who are also in the research direction of Kalman filtering.

1. Why use a Kalman filter?

  
There are many uncertainties in the real society:

1. There is no perfect mathematical model
2. The disturbance of the system is uncontrollable and difficult to model
3. There is an error in the measurement sensor

---

2. Formula derivation

  Let the measurement result be Zk, k means the kth assumption that the real diameter of the coin is 50mm, the data we measured may be Z1=50.1mm, Z2=50.3mm, Z3=49.8mm Normally, we will estimate the real
  data One takes the average (average method is also called mean filter).


  After a series of derivations, we can get:

  current estimated value = last estimated value + Kalman gain × (current measured value - last estimated value) Recursive

   thought: new estimated value is related to last estimated value : The measurement error is the difference between the indicated value of the sensor and the physical quantity actually received by the sensor.

  

  Then, Kalman gain:

  Discussion: The steps to calculate the recursive problem at time k

  are as follows:

---

3. Examples (based on Matlab/Excel platform)

(1) Excel experiment

  Assuming that there are 20 sets of data, we can get:

(2) Matlab experiment

%rand函数: 随机产生0到1内的一个数，rand(50,1)产生50个0到1内的数字
Z = 3*(rand(50,1)*2-1)+50;%产生47到53内的随机数字
x_hat= zeros(50,1);
K    = zeros(50,1);
e    = zeros(50,1);
x_hat(1) = 40;
e(1) = 5;
K(1) = 0;
%%
for k = 2:50
    K(k) = e(k-1)/(e(k-1)+3);
    x_hat(k) = x_hat(k-1)+K(k)*(Z(k)-x_hat(k-1));
    e(k) = (1-K(k))*e(k-1);
end
figure('NumberTitle', 'off', 'Name', 'Recursive Algorithm');
plot(Z);
hold on
plot(x_hat);
legend('测量值','估计值');

  The result of the operation is as follows: