Analysis of Kalman filter algorithm and practical matlab

Kalman filter is an algorithm which uses the state equation of linear system to estimate the state of the system by the input and output data. The optimal estimation can also be regarded as the filtering process because of the influence of the noise and interference of the system.

The core content of the Kalman filter is 5 formulas, which is simple and fast, and is suitable for forecasting and estimating small amount of data.

Here we use an example to illustrate the application of the Kalman algorithm.

Suppose we want to have a car at the T moment whose speed is Vt, the position coordinates for Pt,ut represent the acceleration of the t moment, then we can use the XT to represent the state of the T moment, as follows:

Then we can get the conversion from t-1 moment to T moment, position and velocity as follows:

Use vectors to represent the above conversion process, as follows:

Such as:

Then we can get the following state transition formula:

(1)

Where Matrix F is a state transition matrix, which indicates how the state of the current moment is inferred from the previous state, B is the control matrix, indicating how the control amount U acts on the current matrix, the above formula X has a top hat, which means that the estimate is not optimal.

The state transfer formula can be used to speculate on the current state, but all the speculation is that it contains noise, the greater the noise, the greater the uncertainty, the covariance matrix is used to represent the uncertainties caused by this conjecture.

Covariance matrix

Suppose we have a one-dimensional data, and each measurement is different, we assume that we obey the Gaussian distribution, then we can use mean and variance to represent the dataset, and we project the one-dimensional dataset onto the axis, such as:

As you can see, the one-dimensional data that obeys the Gaussian distribution is mostly distributed around the mean value.

Now let's look at the case where the two-dimensional data that obeys the Gaussian distribution is projected onto the axis, such as:

The two-dimensional data is slightly more complex than one-dimensional data, and after projection there are 3 cases, namely:
Left: Two dimensions of data are unrelated;
Figure: Two-dimensional data positive correlation, which means y increases with X (assuming two dimensions are X and Y, respectively)
Right: Data negative correlation of two dimensions, that is, y decreases with the increase of X.

So how does that show the correlation of data in two dimensions? The answer is the covariance matrix.

State covariance matrix Delivery

In the equation (1), we have obtained the transition formula of the state, but from the above, the covariance matrix of the two-dimensional data is very important to describe the characteristics of the data, then how should we update or pass the covariance matrix of our two-dimensional data? If we use P to represent the state covariance, that is

Then join the state transition matrix F to get

(2)

Also namely:

So we get the conversion formula of covariance.

Now we have two formulas that can be used to predict the current state using these two formulas. According to our normal thinking to understand, the prediction results will not necessarily be correct, there must be errors. And in most of our regression algorithms or quasi-algorithm, the general idea is to predict first, and then see how the prediction results with the actual results of the error, and then adjust the parameters of the prediction function according to this error, and constantly iteratively adjust the parameters until the prediction error is less than a certain threshold value.

The iterative thinking of the Kalman algorithm is similar, but the state X is adjusted according to the error.

Here, our actual data is Z, such as:

Wherein, the matrix H is the parameter of the measurement system, namely the observation matrix, V is the observed noise, and its covariance matrix is R

Then our status update formula is as follows:

where k is the Kalman coefficient, z-hx is the residual, which is what we say, the error between the predicted value and the actual value.

The role of K:

1.K trade-off forecast covariance p and observe covariance matrix R that's more important, believe that the weight of the residuals is small, believe that observation, residual right is significant, by the expression of K can exit this conclusion
2, the expression of the residuals from the observation domain to the state domain (residuals and a scalar, through K to Vector), by the update formula of state X can be obtained by this conclusion.

So far, we've got the best estimate Xt in T state. But in order for our iterative algorithm to persist, we must also update the value of the state covariance.

Update for State covariance

The above is the idea of Kalman filter algorithm, only a simple 5 formula, summarized as follows:

Matlab implementation

 function kalmanfiltering %%Clcclose All%%%% description:kalmanfiltering% Author:liulongpo% time:2015-4-29 16:42:34%%%Z= (1:2: $);% observations the location of the car is the amount we want to modify.Noise=Randn(1, -);Gaussian noise with a% variance of 1Z=z+noise; x=[0 ; 0 ];% Initial Statep=[1 0;  0 1];% State covariance matrixf=[1 1;  0 1];% state Transition matrixq=[0.0001,0;  0 , 0.0001];% State transfer covariance matrixH=[1,0];% Observation MatrixR=1;% observed noise varianceFigure;hold on; for I=1: -% forecast current status based on previous stateX_ = f*x;% update covariance Q system process covariance These two formulas are predictions of the systemP_ = f*p*F '+q;% calculated Kalman gainK = p_*H '/(h*p_*H '+R);% Gets the current state of the optimization estimate gain multiplied by the residualsX = x_+k* (Z (I)-h*x_);% update covariance of K statesP = ( Eye(2)-k*h) *p_;scatter (X (1), X (2),4);% draw point, horizontal axis indicates position, longitudinal axes indicate speedEndEnd

The effect is as follows

Where the x axis is the position and the Y axis is the speed.
In the code, we set the change of X is 1:2:200, then the speed is 2, can be seen, the value after several iterations, the speed is basically swinging around 2, the reason for the wobble is that we added noise.

Let's look at a practical example.

Our data = [149.360, 150.06, 151.44, 152.81,154.19, 157.72];
This is the actual x-axis coordinates of the angular point obtained from the video using the optical flow method, with a total of 6 data, which is the x-coordinate of the continuous 6 frames representing a point. In the next example, we will train with 5 frames of data and then predict the x-axis coordinates of frame 6th.

In the previous MATLAB example, our training data is more, so our initial state is set to [0,0], that is, the position is 0, the speed is 0, in the case of more training data, the initialization data is 0 is not related, because we can see in the above, A short iteration of the algorithm can be useful.

But here, our training data is only 5 frames, so to speed up training, we initialize the position state to the position of the first frame, and the speed is initialized to the difference between the second frame and the first frame.

The code is as follows:

kf.m

 function [preddata,datax] = KF(Dataz) %%%% description:kalmanfiltering% Author:liulongpo% time:2015-4-29 16:42:34%%%Z =Dataz '; len =length(Z);%z= (1:2:200);% observed the position of the car is the amount we want to modify.Noise=Randn(1, Len);Gaussian noise with a% variance of 1Datax =Zeros(2, Len); Z=z+noise; x=[Z (1); Z (2)-Z (1)];% initial states are position and speed, respectivelyp=[1 0;  0 1];% State covariance matrixf=[1 1;  0 1];% state Transition matrixq=[0.0001,0;  0 , 0.0001];% State transfer covariance matrixH=[1,0];% Observation MatrixR=1;% observed noise variance%figure;%hold on; for I=1: Len% forecast current status based on previous state% 2x1 2x1X_ = f*x;% update covariance Q system process covariance These two formulas are predictions of the system% 2x1 2x1 1x2 2x2P_ = f*p*F '+q;% calculated Kalman gainK = p_*H '/(h*p_*H '+R);% Gets the current state of the optimization estimate gain multiplied by the residualsX = x_+k* (Z (I)-h*x_);% update covariance of K statesP = ( Eye(2)-k*h) *p_;datax (:,I) =[X (1); X (2)];%scatter (x (1), X (2), 4),% draw point, horizontal axis indicates position, vertical shaft indicates speedEndPreddata = f*x;End

testkf.m

 function testkf %% Clcclose All%%%data = Load (' d:\\a.txt ');%data = [149.360, 150.06, 151.44, 152.81,154.19,157.72,157.47,159.33,153.66];data =[149.360 , 150.06, 151.44, 152.81,154.19 ,157.72];[Preddata, Datax]= KF (Data '); error = Datax (1,:)-data;I=1:length(data); Figuresubplot311Scatter (I, data,3), title (' raw data ') subplot312Scatter (I, Datax (1,:),3), title (' Predictive data ') subplot313Scatter (I, Error,3), title (' Predictive error ') Preddata (1)%{Scatter (I, Error,3); Figurescatter (I, data,3) Figurescatter (I, Preddata (1,:),3)%}End

The effect is as follows:

The prediction result is: 155.7493, with the actual result 157.72 only 1.9 error, you can see that the Kalman filter algorithm for a small amount of data prediction effect is quite good. Of course, we also got the prediction speed at the same time we predicted the position.

Reference: Video: The principle of Kalman filter and its implementation in MATLAB

Analysis of Kalman filter algorithm and practical matlab