Kalman Filter (reproduced)

Before learning the Kalman filter, first look at why it is called "Kalman". Like other famous theories (such as Fourier transform, Taylor series, etc.), Kalman is also a person's name, and unlike them, he is a modern man!

Kalman full name Rudolf Emil Kalman, Hungarian mathematician, was born in 1930 in Budapest, the Hungarian capital. 1953,1954 holds a bachelor and Master's degree in electrical Engineering from MIT respectively. He received his doctorate from Columbia University in 1957. The Kalman filter We are going to learn is the result of his doctoral dissertation and the 1960 paper "A New Approach to Linear Filtering and prediction Problems" (a novel method of linear filtering and prediction problems). If you are interested in this paper, you can download the address here:http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf

In simple terms, the Kalman filter is a "optimal recursive data processing algorithm (optimal autoregressive data processing algorithm)". He is the best, the most efficient and even the most useful solution to a large part of the problem. He has been widely used for more than 30 years, including robotic navigation, control, sensor data fusion and even military radar systems, as well as missile tracking and more. In recent years, it has been applied to computer image processing, such as head face recognition, image segmentation, Image edge detection and so on.

2. Introduction of Kalman Filter
(Introduction to the Kalman Filter)

In order to understand the Kalman filter more easily, it will be explained by the description of the image, rather than a large number of mathematical formulae and mathematical symbols as in most reference books. However, his 5 formula is the core of the content. In combination with modern computers, the Kalman program is quite simple, as long as you understand his 5 formulas.

Before introducing his 5 formulas, let's start with the following example, step-by-step exploration.

Let's say we're going to study the temperature of a room. Judging from your experience, the temperature of the room is constant, that is, the temperature of the next minute is equal to the current minute of the temperature (suppose we take a minute to do the time unit). Assuming that you are not 100% convinced of your experience, there may be a few degrees up or down deviations. We consider these deviations to be Gaussian white noise (Gaussian Noise), which means that the time is not related to these deviations and is in accordance with the Gaussian distribution (Gaussian distribution). In addition, we put a thermometer in the room, but the thermometer is not accurate, the measured value will be more than the actual value deviation. We also consider these deviations as Gaussian white noises.

Okay, now for a minute we have two values for the temperature of the room: the predicted value of your experience (the predicted value of the system) 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 do you 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). It's amazing that he can change his own values with different moments.

Let's discuss the Kalman on the real engineering system.

3. Kalman Filter algorithm
(The Kalman Filter algorithm)

In this section, we describe the Kalman filter originating from Dr Kalman. The following description involves some basic conceptual knowledge, including the probability (probability), the variable (random Variable), the Gaussian or normal distribution (Gaussian distribution), and the State-space model. However, the detailed proof of the Kalman filter can not be described here.

First, we need to introduce a system of discrete control processes first. The system can be described by a linear stochastic differential equation (Linear Stochastic difference equation):
X (k) =a x (k-1) +b U (k) +w (k)
Plus the measured value of the system:
Z (k) =h X (k) +v (k)
In the last two equations, X (k) is the system state of the K-moment, and U (k) is the control amount of the system at K-time. A and B are system parameters, and for multi-model systems, they are matrices. Z (k) is the K-time measurement, H is the parameter of the measurement system, for the multi-measurement system, H is the matrix. W (k) and V (k) represent the process and measured noise, respectively. They are assumed to be Gaussian white noise (Gaussian Noise), and their covariance are q,r (here we assume they do not change with the system State).

For satisfying the above conditions (linear stochastic differential system, process and measurement are Gaussian white noise), the Kalman filter is the optimal information processor. Let's use them to combine their covariances to estimate the optimal output of the system (similar to the one in the previous section).

First, we need to use the system's process model to predict the next state of the system. Assuming that the current system state is k, depending on the system's model, it can be predicted to appear in the state based on the previous state of the system:
X (k|k-1) =a x (k-1|k-1) +b U (k) ... (1)
Formula (1), X (k|k-1) is the result of the previous state prediction, X (k-1|k-1) is the optimal result of the previous state, U (k) is the current state of control, if there is no control amount, it can be 0.

The results of our system have been updated so far, but the covariance corresponding to X (K|K-1) has not been updated. We use p to denote covariance:
P (k|k-1) =a P (k-1|k-1) A ' +q ... (2)
Formula (2), P (K|k-1) is x (k|k-1) corresponding to the Covariance,p (k-1|k-1) is x (k-1|k-1) corresponding to the Covariance,a ' represents a transpose matrix, Q is the covariance of the system process. The first two of the 5 formulas of the Kalman filter is the prediction of the system.

Now that we have the predicted results of the present state, we then collect the measured values of the current state. Combining the predicted values with the measured values, we can get the optimal estimate of the current state (k) x (k|k):
X (k|k) = x (k|k-1) +kg (k) (Z (k)-H X (k|k-1)) ... (3)
Where KG is the Kalman gain (Kalman Gain):
Kg (k) = P (k|k-1) H '/(H P (k|k-1) h ' + R) ... (4)

So far, we've got the best estimate X (k|k) in K state. But in order for the other Kalman filter to run continuously until the system process is over, we also need to update the K-state x (K|K) Covariance:
P (k|k) = (i-kg (k) H) p (k|k-1) ... (5)
Where I is the matrix of 1, for single model single measurement, I=1. When the system enters the k+1 state, p (k|k) is the P (k-1|k-1) of the equation (2). In this way, the algorithm can be calculated from the regression.

The principle of Kalman filter basically describes, the formula 1,2,3,4 and 5 is his 5 basic formulas. According to these 5 formulas, it is easy to implement the computer program.

Below, I will use the program to give an example of the actual run ...

4. Simple example
(A simple Example)

Here we combine the second and third section, to give a very simple example to illustrate the work of the Kalman filter. The example given is to further describe the example of section II, and it will also be equipped with a program simulation result.

As described in section II, consider a room as a system and then model the system. Of course, the models we see don't need to be very precise. The temperature of the room we know is the same as the temperature of the previous moment, so a=1. There is no control amount, so u (k) = 0. It is therefore concluded that:
X (k|k-1) =x (k-1|k-1) ..... (6)
The formula (2) can be changed to:
P (k|k-1) =p (k-1|k-1) +q ... (7)

Because the measured value is a thermometer, which corresponds directly to the temperature, so h=1. The formula 3,4,5 can be changed to the following:
X (k|k) = x (k|k-1) +kg (k) (Z (k)-X (k|k-1)) ... (8)
Kg (k) = P (k|k-1)/(P (k|k-1) + R) ... (9)
P (k|k) = (1-kg (k)) p (k|k-1) ... (10)

Now we simulate a set of measured values as input. Assuming the true temperature of the room is 25 degrees, I simulated 200 measurements, the average of which was 25 degrees, but added a Gaussian white noise with a standard deviation of several degrees (blue line in the picture).

In order to get the Kalman filter to work, we need to tell the initial value of the Kalman two 0 moments, which is X (0|0) and P (0|0). Their values don't care too much, just give it to one, because with the work of the Kalman, X will gradually converge. But for P, generally do not take 0, because this may make the Kalman fully believe that your given x (0|0) is the optimal system, so that the algorithm can not converge. I chose X (0|0) =1 degrees, P (0|0) = 10.

The real temperature of the system is 25 degrees, which is shown in black line. The red line in the graph is the optimal result of the Kalman filter output (the result is set q=1e-6,r=1e-1 in the algorithm).




The following Kalman filter program is attached to MATLAB:

Clear
n=200;
W (1) = 0;
W=RANDN (1,n)
X (1) = 0;
A=1;
For K=2:n;
X (k) =a*x (k-1) +w (k-1);
End


V=RANDN (1,n);
Q1=STD (V);
rvv=q1.^2;
Q2=STD (x);
rxx=q2.^2;
Q3=STD (w);
rww=q3.^2;
c=0.2;
Y=c*x+v;

P (1) = 0;
S (1) = 0;
For T=2:n;
P1 (t) =a.^2*p (t-1) +rww;
B (t) =c*p1 (t)/(C.^2*P1 (t) +rvv);
S (t) =a*s (t-1) +b (t) * (Y (t)-a*c*s (t-1));
P (t) =p1 (t)-c*b (t) *p1 (t);
End

T=1:n;
Plot (t,s, ' R ', T,y, ' G ', t,x, ' B ');

Transferred from: http://carpa.bokee.com/4725695.html

Kalman Filter (reproduced)