[Post] Kalman Filter

I have been here for a few months and found that many people are interested in some problems. Therefore, we hope to do our best to discuss some algorithms as far as possible. Now let's talk about Kalman filter. If time and capability permits, I want to write other algorithms, such as genetic algorithms, Fourier transformation, digital filtering, neural networks, and image processing.
Because we cannot write complex mathematical formulas here, we can only describe images. If anyone is an expert in this area, you are welcome to discuss the corrections.

Kalman Filter-Kalman Filter

1. What is a Kalman filter?
(What is the Kalman Filter ?)

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 to the address here: http://www.cs.unc.edu /~ Welch/Media/pdf/kalman1960.pdf.

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)

To make it easier to understand the Kalman filter, the image description method will be used here to explain, rather than listing a lot of mathematical formulas and mathematical symbols like most reference books. However, his five formulas are the core content. 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 explore them step by step 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. The system can be described by a linear random differential equation (Linear Stochastic difference equation:
X (K) = a x (k-1) + B u (k) + W (k)
Plus the system measurement value:
Z (K) = H x (k) + V (k)
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. Z (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. W (K) and V (k) represent the 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. Next we will use them together with their covariances to estimate the optimal output of the system (similar to the temperature example in the previous section ).

First, we need to use the system process model to predict the next state system. Assume that the current system status is K. Based on the system model, you can predict the status based on the previous status of the system:
X (k | k-1) = a x (K-1 | k-1) + B u (k )........... (1)
In formula (1), x (k | k-1) is the result of using the previous state prediction, x (K-1 | k-1) is the result of the optimum of the previous State, u (k) the current State control. If there is no control, it can be 0.

Our system results have been updated so far, but the covariance for X (k | k-1) has not been updated yet. We use P to represent covariance:
P (k | k-1) = a p (K-1 | k-1) a' + q ......... (2)
In formula (2), P (k | k-1) is the covariance of X (k | k-1), P (K-1 | k-1) is the covariance of X (K-1 | k-1, a' indicates the transpose matrix of A, and Q is the covariance of the system process. Formula 1 and 2 are the first two of the five formulas of Kalman filter, that is, the prediction of the system.

Now we have the prediction result of the current state, and then we collect the measurement value of the current state. Combined with the predicted values and measured values, we can obtain the optimal estimation value 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 Kalman gain (Kalman gain ):
Kg (K) = P (k | k-1) H '/(h p (k | k-1) H' + r )......... (4)

So far, we have obtained the optimal estimate value x (k | K) in K State ). But to keep the other Kalman filter running until the system process ends, we need to update the covariance of X (k | K) in K State:
P (k | K) = (I-kg (k) H) P (k | k-1 )......... (5)
Where I is a matrix of 1, for a single measurement of a single model, I = 1. When the system enters the k + 1 State, P (k | K) is the formula (2) P (K-1 | k-1 ). In this way, the algorithm can be used to perform self-regression operations.

The principle of Kalman filter is basically described. Formula 1, 2, 3, 4, and 5 are his five basic formulas. According to these five formulas, it is easy to implement computer programs.

Next, I will use a program to give an example of actual running...
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.

According to the description in section 2, the room is regarded as a system, and then the system is modeled. 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 ).

Reprinted from: http://www.china-vision.net/blog/user1/6/archives/2006/2006111115624.shtml