The principle of Kalman filter algorithm and its application in a prediction problem (MATLAB)

principle

The core of the Kalman filter algorithm is the five formulas. These five formulas include the measured value, a priori value, and a posterior value. The prior value is actually an approximate value calculated according to the system model (the modeling is inaccurate), and at the same time, we can use the testing equipment to measure a measurement value. The problem now is how to fuse these two values ​​to calculate A posterior value at this moment, this posterior value needs to meet the principle of minimum error variance (at this time, it is closest to the true value). The Kalman filtering algorithm is a mathematical optimization problem that has been rigorously deduced. It will not be deduced here. The five formulas of the final algorithm are given: X ^ 1, k − \hat{X}^-_{1,k} in the

formula${\hat{X}}_{1,k}^{-}$Is the prior position, ${\hat{X}}_{1,k}$Is the posterior value (the result of Kalman filtering in the k-th step). In Kalman filtering, several data need to be initialized, namely X at time 0 $\hat{X}$ AndP k P_kat time 1$P_k$, Then the prior value can be calculated according to the first formula at time 1, and the new P k − P^-_k can be calculated according to the second formula$P_k^{-}$(A priori), then K k K_k is calculated according to the third formula$K_k$(The right side of this formula is $P_k^{-}$Wrong), the fourth formula performs data fusion of the prior value and the measured value, and the last formula updates $P_k$。

MATLAB implements a Kalman filter example

Here we assume that an object is moving in one-dimensional space, with two state variables, speed and position. Initially at position 0, the speed is 1, and the system sampling period is 1s. The existing satellite observer observes its position and speed, but both exist. Noise, we have already modeled this second-order model (position at time k is position at time k-1 + speed*1s + noise, and the speed at time k is speed at time k-1: + noise), and now we use Kalman filter algorithm to filter .

code show as below:

%------------------Kalman filter----------------%
%This is an application of Kalman filter algorithm in a simple problem which needs
%us to estimate the displacement and speed. The initial condition is [0;1], which 
%means the x0 = 0, v0 = 1. Besides, the interval time is 1s. p(w) and p(v) are the noise
%which obey normal distribution and the covariance matrices are Q and R respectively.

H = eye(2); A = [1 1;0 1];
Pk = eye(2); Q = 0.1*eye(2);R = eye(2);
X_k = [0;1]; X_hat = [0;1];

X_kdata = [0;1]; X_hatdata = [0;1];
X_hat_data = []; Z_kdata = [];
iter = 20;
for i = 1:iter
    pw = mvnrnd([0 0],[0.1 0;0 0.1],1)';
    pv = mvnrnd([0 0],[1 0;0 1],1)';
    X_k = A*X_k + pw; X_kdata = [X_kdata,X_k];
    Z_k = H*X_k + pv; Z_kdata = [Z_kdata,Z_k];
    
    Pk_ = A*Pk*A'+Q;
    Kk = Pk_*H'*inv(H*Pk_*H'+R);
    X_hat_ = A*X_hat; 
    X_hat_data = [X_hat_data,X_hat_];
    X_hat = X_hat_ + Kk*(Z_k - H*X_hat_);
    X_hatdata = [X_hatdata,X_hat];
    Pk = (eye(2) - Kk*H)*Pk_;
end

figure(1);
plot(0:iter,X_kdata(1,:),0:iter,X_hatdata(1,:)...
    ,1:iter,X_hat_data(1,:),1:iter,Z_kdata(1,:)...
    ,0:iter,X_kdata(2,:),0:iter,X_hatdata(2,:)...
    ,1:iter,X_hat_data(2,:),1:iter,Z_kdata(2,:));
legend("实际位置","后验位置","先验位置","测量位置",...
    "实际速度","后验速度","先验速度","测量速度"...
    ,"Location","best");
grid on; title("Kalman filter");
xlabel("iter"); ylabel("x or v");

• result
  
• Partial zoom
  

It can be seen that the Kalman filter is actually a data fusion process. The measurement is biased to the calculation when the calculation is inaccurate, and the calculation is biased to the measurement when the measurement is inaccurate. Fusion coefficient $K_k$It is obtained by optimizing the error variance.