Kalman Filter Model and Its MATLAB implementation

Kalman filter is built onHidden Markov ModelIs a recursive estimation. That is to say,You only need to know the estimated value of the previous State and the observed value of the current State to calculate the optimal estimated value of the current state.

You do not need earlier historical information.

 

Two statuses of Kalman Filter

1. Optimal Estimation

2. error covariance matrix

These two variable iterations have no influence on the initial values. In the end, all data can converge to the optimal estimation.

 

Prediction process

F is the state transition matrix, and B is the control matrix (or not required ). Q is the covariance of process noise.

Here, the small check mark on the left of the equation indicates the estimator. There is a negative sign indicating that this estimator is not optimal and almost something.

 

Update process

It is a State update process. H is the measurement matrix, z is the observation matrix, and the measurement residual is in the brackets.

The formula 2nd is the Kalman gain matrix. R is the covariance matrix of observed noise.

It is the update process of the error covariance matrix.

 

So we can start iteration. We take the car's [location speed] as the state variable, and the car is moving at a constant speed.

Z = () + 0.1 * randn (1,100); % white noise x = [0.8; 1.2]; % initial optimal estimation state p = [1.2 0.9; 0.8 1.3]; % initial optimal covariance matrix F = [1 1; 0 1]; % state transfer matrix Q = [0.001 0; 0 0.001]; % Predicted Noise covariance matrix H = [1 0]; % observation matrix R = 1; % observed noise covariance matrix hold onfor I = X _ = f * X; % There is no control variable P _ = f * p * F' + q; k = P _ * H'/(h * P _ * H' + r ); X = X _ + K * (Z (I)-H * X _); P = (eye (2)-K * h) * P _; plot (X (1), X (2), '*'); End
 

We can see that although the initial statusWrite at willBut it quickly converges to the near real value. Prediction noise covariance matrix Q needsSmallerThis indicates that we have sufficient confidence in the status transition matrix. Otherwise, the prediction results will be poor.

welcome to the discussion and follow up This blog and Weibo and zhihu personal homepage subsequent content continues to be updated ~

Reprinted, please respect the work of the author and keep it completelyThe preceding textAndArticle LinkThank you for your support!