Principle and example of Kalman filter

At present, we can find the understanding of the Kalman filter almost only about the temperature of the example, the specific implementation of almost no, or is not clear, this paper mainly on the Kalman filter in the yard, and give the program to achieve easy to understand. Or an example of a temperature change:
For a minute we have two values for the temperature of the room: Your predicted value based on experience (system's predicted value) and the value of the thermometer (measured value). We will use these two values to estimate the actual temperature value of the room in combination with their respective noises.

If we were to estimate the actual temperature value of the K-moment. First you have to predict the temperature of the K-moment according to the temperature of the k-1 moment. Because you believe that the temperature is constant, so you will get the temperature of the K-moment is the same as the k-1 moment, assuming 23 degrees, while the value of the Gaussian noise deviation is 5 degrees (5 is obtained: If the k-1 time estimate of the optimal temperature value deviation is 3, you predict the uncertainty is 4 degrees, They square the sum and then prescribe, is 5). Then you get the temperature value of the K-moment from the thermometer, assuming it is 25 degrees, and that the deviation of the value is 4 degrees.

Since we used to estimate the actual temperature at k time there are two temperature values, respectively 23 degrees and 25 degrees. How much is the actual temperature? Believe in yourself or believe in a thermometer. We can use their covariance to judge who believes a little more. Because kg^2=5^2/(5^2+4^2), so kg=0.78, we can estimate the actual temperature value of K time is: 23+0.78* (25-23) =24.56 degrees. As can be seen, because the covariance of the thermometer is relatively small (more believe the thermometer), so the estimated optimal temperature value bias thermometer value.

Now that we've got the optimal temperature for the K-moment, the next step is to enter the k+1 moment and make a new optimal estimate. So far, it seems that something has not been seen coming back. Well, before we enter the k+1 moment, we have to figure out the deviation of the optimal value (24.56 degrees) of the K-moment. The algorithm is as follows: ((1-kg) *5^2) ^0.5=2.35. Here's 5 is the above K time you predict the 23 degrees of temperature deviation, the 2.35 is to enter the k+1 time after the K time estimate of the optimal temperature value deviation (corresponding to the above 3).

In this way, the Kalman filter constantly recursive covariance, thus estimating the optimal temperature value. He runs fast, and it retains only the covariance of the last moment. The above kg is the Kalman gain (Kalman Gain).

Specific implementation:

Define the required variables first: The process noise, the measured noise, and the covariance values are all based on the actual conditions of double kf_q = 8;      Q: Process noise, Q increases, dynamic response becomes faster, convergence stability becomes bad double kf_r = 8;     R: Measurement noise, r increase, dynamic response slow, convergence stability good double kf_a = 1;    System parameters, set to 1 double x_next;    The system value of the next moment is double z_next;    The measured value of the next moment is double x_now;    The system value of this moment is double z_now;  The measured value of this moment is double x_bestlast;  The optimal value of the last moment double x_bestnow;  The optimal value of this moment is double x_bestnext; The optimal value of the next moment double p_last=8;//the covariance of the previous moment, that is, the deviation of the optimal value double p_now;//the covariance of the current moment, i.e. the deviation of the optimal value double kg;//the deviation of the Gaussian noise double  Kf_covoriance; Deviation value//Kalman filter//Calculate the optimal value of this moment x_bestlast = 23; Since there is no system value at the last moment, the measured value is 23 degrees as the optimal value X_now = X_bestlast;    Assigns the optimal value of the last moment to the system value at this moment Z_now = secondkf.at (2*i + 4); The measured value for this moment is kg = sqrt (p_last*p_last + kf_q * kf_q);//estimated covariance deviation kf_covoriance = sqrt ((kg * kg)/(kg * kg + kf_r * KF) _r)); Calculates deviation values based on covariance and process noise X_bestnow = X_now + kf_covoriance * (Z_now-x_now);
   Calculate the optimal value of this moment//calculate the optimal value of the next moment, that is, the predicted track value after Kalman filter x_next = X_bestnow; Z_next = Z_now;  P_now = sqrt ((1-kf_covoriance) *kg*kg);  Calculate the deviation of the optimal value for the next moment kg = sqrt (p_now*p_now + kf_q * kf_q);//estimate covariance deviation kf_covoriance = sqrt ((kg * kg)/(kg * kg + kf_r) * kf_r)); Calculates deviation values based on covariance and process noise X_bestnow = X_next + kf_covoriance * (Z_next-x_next); Calculate the optimal value of the next moment, that is, the predicted track value after Kalman filter