Understanding of Kalman Filter

1. Introduction (Brief Introduction)

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

Kalman full name Rudolf Emil Kalman, a Hungarian mathematician, was born in Budapest, Hungary on 1930. In, he obtained a bachelor's degree and a Master's degree in electrical engineering from MIT. In 1957, he obtained a doctorate from Columbia University. The kalman filter we want to learn now, it is because of his doctoral thesis and the paper a new approach to linear filtering and prediction problems published in 1960 (a new method for linear filtering and prediction ). If you are interested in this paper, you can download it:

Http://www.cs.unc.edu /~ Welch/Kalman/Media/pdf/kalman1960.pdf

What is Kalman filter? Let's take a look at the explanation on the Wiki:

A typical example of Kalman filter is to predict the coordinates and velocity of an object's position from a finite set of observed sequences (which may be deviated) that contain noise. It can be found in many engineering applications (such as radar and computer vision. At the same time, Kalman filter is also an important topic in the Control Theory and Control System Engineering. For example, radar is interested in tracking targets. However, the measured values of the target location, speed, and acceleration often have noise at any time. Kalman filter uses the dynamic information of the target to remove the noise and obtain a good estimation of the target location. This estimation can be an estimation of the current target location (filtering), an estimation of the future location (prediction), or an estimation of the past location (interpolation or smoothing ).
Stanley Schmidt first implemented the Kalman filter. Kalman found his approach useful in solving the Orbit Prediction of the Apollo program during a visit to the NASA James Research Center, which was then used by the navigation computer of the Apollo spacecraft. The papers on this filter are published by swerling (1958), Kalman (1960), and Kalman and BuCy (1961.
At present, Kalman filtering has many different implementations. The form first proposed by Kalman is now generally called simple Kalman filter. In addition, there are also Schmidt extension filters, information filters, and many Bierman, Thornton developed square root filter variants. Perhaps the most common Kalman filter is the phase-locked loop, which is widely used in radio, computer, and almost any video or communication equipment.

Simply put, the Kalman filter is an "optimal recursive Data Processing Algorithm (optimized autoregressive Data Processing Algorithm )". It is the best, the most efficient, and even the most useful solution to most problems. He has been widely used for more than 30 years, including robot navigation, control, sensor data fusion, and even military radar systems and missile tracking. In recent years, it has been applied to computer image processing, such as face recognition, image segmentation, and image edge detection.

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

In order to better understand the Kalman filter, we first use the image description method to explain it. Then, we further explain and explore it based on the five core formulas. Combined with modern computers, the Kalman program is actually quite simple, as long as you understand his five formulas.

Before introducing the five formulas, let's give an intuitive explanation based on the example below.

Suppose we want to study the temperature of a room. According to your experience, the temperature in this room is constant, that is, the temperature in the next minute is equal to the current temperature in this minute (assuming that we use one minute as the unit of time ). If you do not believe in your experience for 100%, there may be up or down deviations. We regard these deviations as white Gaussian noise, that is, these deviations have no relationship with the time before and after and comply with the Gaussian distribution (Gaussian distribution ). In addition, we put a thermometer in the room, but this thermometer is not accurate, the measured value will be different from the actual value. We also regard these deviations as Gaussian white noise.

Well, now we have two temperature values for the room for a certain minute: Your empirical predicted values (system predicted values) and the thermometer values (measured values ). Next we will use these two values in combination with their respective noise to estimate the actual temperature of the room.

Assume that we want to estimate the actual temperature at K time. First, you need to predict the temperature at K time based on the temperature value of the K-1 moment. Because you believe that the temperature is constant, so you will get the K-moment temperature forecast is the same as the K-1 moment, assuming 23 degrees, at the same time, the deviation of the Gaussian noise value is 5 degrees (5 is obtained as follows: if the deviation of the optimum temperature value estimated at the K-1 moment is 3, your uncertainty about your prediction is 4 degrees, and they are square and then square, that is, 5 ). Then, you get the temperature value at K moment from the thermometer, which is assumed to be 25 degrees, and the deviation of this value is 4 degrees.

The actual temperature used to estimate K-moment has two temperature values: 23 degrees and 25 degrees. What is the actual temperature? Believe in yourself or believe in a thermometer? We can use their covariance to determine who believes more. Because kg ^ 2 = 5 ^ 2/(5 ^ 2 + 4 ^ 2), so kg = 0.78, we can estimate the actual temperature value of K moment is: 23 + 0.78 * (25-23) = 24.56 degrees. It can be seen that because the covariance of the thermometer is relatively small (I believe the thermometer), the estimated optimal temperature value is biased towards the value of the thermometer.

Now we have obtained the optimal temperature value at K moment. The next step is to enter k + 1 moment for a new optimal estimate. So far, it seems that nothing about auto-regression has appeared. By the way, before entering the k + 1 moment, we need to calculate the deviation of the optimal value (24.56 degrees) at the K moment. The algorithm is as follows: (1-kg) * 5 ^ 2) ^ 0.5 = 2.35. 5 here is the deviation of the 23-degree temperature value you predicted at K time above, the resulting 2.35 is the deviation of the optimal temperature value estimated after the k + 1 moment (corresponding to 3 above ).

In this way, the Kalman Filter continuously recursion covariance to estimate the optimal temperature value. It runs very fast, and it only keeps the covariance of the previous moment. The above kg is the Kalman gain (Kalman gain ). He can change his value at different times. Is it amazing!

Let's get down to the truth and discuss the Kalman in the real engineering system.

3. Kalman Filter Algorithm (the Kalman Filter Algorithm)

In this section, we will describe the Kalman Filter derived from Dr Kalman. The following description involves some basic conceptual knowledge, including probability, random variable, Gaussian or normal distribution, and state-space model. However, the detailed proof of the Kalman filter cannot be described here.

First, we need to introduce a discrete control process system. This system can be described by a linear random differential equation (Linear Stochastic difference equation). We will explain it in the following PPT:

In the previous two formulas, x (k) is the system state at K moment, and U (k) is the control value for the system at K moment. A and B are system parameters. For multi-model systems, they are matrices. Y (k) is the measured value at K time, and H is the parameter of the measurement system. H is the matrix of multiple measurement systems. Q (K) and R (k) represent process and measurement noise respectively. They are assumed to be white Gaussian noise and Their covariance is Q and R (Here we assume they do not change with the system status ).

Kalman filter is the optimal information processor to meet the above conditions (process and measurement of linear random differential systems are Gaussian white noise. The following describes the process of the KF algorithm and the five core update equations:

KF Algorithm

Five updated equations are:

It is inconvenient to write formulas, so I wrote a PDF file and pasted it below. The following describes the Kalman Algorithm Based on the example above and the five core formulas:

In this way, the algorithm can be used to perform self-regression operations.

The clever guy may have seen it and asked him why Kalman gain was as expected in step 3. Now I just want to give a rough look at the principle. The details are complicated, if you are interested, you can refer to this document to push it.
Remember the error covariance matrix $ P_k $ we mentioned above, that is, the error covariance matrix for the k-th optimal temperature, which corresponds to 3 and 2 in the above example. 35 .... these values. In the following PPT, we can minimize P to obtain the Kalman gain K. corresponding to the above example, K is used to minimize the deviation of the optimal temperature value, that is, to minimize P (k ):

We can see from step 4 that the optimal temperature value of the K-Moment System = The estimated state value of the K-1 moment (by the optimum temperature value of the previous state plus the process error) + deviation with Kalman gain weight. If the observed error is far greater than the estimated error, K is very small, and the predicted value of K is approximately equal to the state estimation value of K. If the error of the state estimation value of I is far greater than the observed error, at this time, the corresponding Q is large, K is large, and the I-moment state estimation value is more inclined to observe the data.

The basic description of the Kalman Filter Principle is complete, hoping to help you understand these five formulas, and its algorithm can be easily implemented by computer programs. Next, I will use a program to give an example of actual operation.

4. Simple example (a simple example)

Here we use section 3 of section 2 to give a very simple example to illustrate the operation process of Kalman filter. The example above is a further example of section 2, which is also matched with a program simulation result.

Root section 2 describes the room as a system, and then models the system. Of course, the model we see does not need to be very accurate. The temperature of the room we know is the same as that of the previous time, so a = 1. There is no control, so u (K) = 0. Therefore, it is concluded that:
X (k | k-1) = x (K-1 | k-1 )......... (6)
The sub-Statement (2) can be changed:
P (k | k-1) = P (K-1 | k-1) + q ......... (7)

Because the measured value is a thermometer and corresponds directly to the temperature, H = 1. Format 3, 4, and 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 measurements as input. Assuming that the actual temperature in the room is 25 degrees, I have simulated 200 measurements with an average value of 25 degrees, however, the white Gaussian noise with a standard deviation of several degrees is added (Blue Line in the figure ).

In order for the Kalman filter to start working, we need to tell the initial values of the Kalman two zero-hour moments: x (0 | 0) and P (0 | 0 ). They don't need to care too much about their values. Just give them one, because X will gradually converge with Kalman's work. However, for P, do not take 0, because it may make Kalman fully believe that the x (0 | 0) You have given is the optimal system, so that the algorithm cannot converge. I selected x (0 | 0) = 1 degree, P (0 | 0) = 10.

The actual temperature of the system is 25 degrees, which is expressed by a black line in the figure. The red line in the figure is the optimal result output by the Kalman Filter (q = 1e-6, r = 1e-1 is set in the algorithm ).

The kalman filter program under MATLAB is attached:

Clear n = 200; W (1) = 0; % W process noise W = randn (1, N) x (1) = 25; A = 1; % A is a (k) for k = 2: N; X (K) = A * X (k-1) + W (k-1); End v = randn (1, n); % V indicates the observed noise Q1 = STD (V); rvv = Q1. ^ 2; Q2 = STD (x); rxx = q2. ^ 2; q3 = STD (w); rww = Q3. ^ 2; C = 0.2; % C indicates H (k) y = C * x + V; % Y is the observed value P (1) = 0; S (1) = 0; for t = 2: N; P1 (t) =. ^ 2 * P (t-1) + rww; % P1 is the square thread, P '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 ');

For more detailed procedures, refer to relevant materials.

The article references:

1 blog http://hi.baidu.com/irvkqscjezbrtwq/item/4ad3bb018b8c7e37a3332a07

2 automatic Institute Dong qiulei class Courseware

3 learning opencv in Shiqi sans 4 Kalman Filter

4 if you do Video Tracking specific parameter selection can refer to "Digital Video Processing" Li hongsong P102-106

5 if you want to explore its specific derivation process, refer to modern signal processing, Zhang xianda P177-188