A list,

What is Kalman filter Kalman filter is suitable for estimating the optimal state of a dynamic system. Even if the observed system state parameters contain noise and the observed value is not accurate, kalman filter can also complete the optimal estimation of the true value of the state. Most of the tutorials on the web are about The derivation of Kalman’s mathematical formulas, and it’s hard to keep track of the main lines and ideas. So I referred to a foreign scholar’s article, on the working principle of Kalman filter, and then wrote a small program based on OpenCV to explain.

2 What can Kalman filter do

Let’s say we have a DIY mobile car. The car looks like this:



The car can move in the wilderness, and in order to control it, you need to know where it is and how fast it is moving. So, build a vector to store the position and speed of the car



In fact, there are many states of a system, and it is important to choose the state you care most about to build this state vector. States, for example, include the level of water in a reservoir, the temperature of a blast furnace in a steel mill, the position of a fingertip on a tablet screen, and so on, all of which require constant tracking. All right, back to business, the car has a GPS sensor on it, and this sensor has an accuracy of 10 meters. But if there are rivers and cliffs in the wilderness where the car is driving, 10 meters is too wide to fall into and not be able to continue working. Therefore, GPS positioning alone is unable to meet the demand. In addition, if someone says that the car itself receives the movement instructions sent by the controller, according to the number of laps the wheel has turned, it can know how far it has gone, but the direction is unknown, and the phenomenon of the car slipping and wheel idling on the road is absolutely inevitable. Therefore, GPS and the code plate of the motor on the wheel and other sensors indirectly provide us with the information of the car, which contains a lot of and uncertainty. If you put all this information together, can you compute the exact information we want? The answer is yes!



3. Working principle of Kalman filter

1. Prior state estimation

Take the state variable we created earlier,



The figure below shows a state space graph. For the convenience of study, if the car is running on an absolutely straight line, the direction of its position and speed is determined, while the size is uncertain.





Ii. Source code

clear; clc; % Number of sampling points N=228; % test data: latitude latitude=load('C:\Users\ Lenovo \Desktop\ Motion trajectory prediction based on MATLAB \latitude.txt'); % Real dimension value lat=latitude; % Kalman filter processing status, i.e., estimated value lat_kf=zeros(1,N); % lat_z=zeros(1,N);
P=zeros(1,N); % Initial latitude lat(1) =29.8131; % Initial covariance P(1) =0.09; % Initial value lat_z(1) =29.8027; % Initial estimated state. Lat_kf (1)=lat_z(1); % Noise variance % System noise variance Q=0.1; % Measurement noise variance R=0.001; The % variance determines the noise size W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N); % system matrix F=1;
G=1;
H=1; % The system status is1D I = eye (1); % simulate latitude measurement and filteringfor k=2:N % The true latitude at the time %k is unknown to the forecasting instrument. The reported value may be infinitely close to the true value, but not the true value %lat(k)=F*lat(k)- 1)+G*W(k- 1); % latitude lat_z(k)=H*lat(k)+V(k) at time K; The %kalman filter % has the measured values lat_z(k) and k at time K- 1% state prediction lat_pre=F*lat_kf % k- 1); % covariance prediction P_pre=F*P(k)- 1)*F'+Q; % Calculate kalman gain Kg=P_pre* INV (H*P_pre*H)'+R); New interest e = % lat_z (k) - H * lat_pre; % status update lat_kf(k)=lat_pre+Kg*e; % covariance update P(k)=(i-kg *H)*P_pre; End % Calculation error % Error between measured value and true value Err_Messure=zeros(1,N); % Error between kalman estimate and true value Err_Kalman=zeros(1,N);
for k=1:N
    Err_Messure(k)=abs(lat_z(k)-lat(k));
    Err_Kalman(k)=abs(lat_kf(k)-lat(k));
end
t=1:N;
Copy the code

3. Operation results



Fourth, note

Version: 2014a complete code or write plus 1564658423