Understanding Kalman Filter in 30 Minutes
The original code of Kalman filter is available at the end of the article, please download by yourself
1 . Brief Introduction
Before learning the Kalman filter, let's first take a 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, but unlike them, he is a modern person!
Kalman's full name is Rudolf Emil Kalman, a Hungarian mathematician, born in Budapest, Hungary in 1930. In 1953, he received a bachelor's degree and a master's degree in electrical engineering from the Massachusetts Institute of Technology in 1954. He received his Ph.D. degree from Columbia University in 1957. The Kalman filter we are going to learn now comes from his doctoral dissertation and the 1960 thesis "A New Approach to Linear Filtering and Prediction Problems" (a new method for linear filtering and prediction problems). If you are interested in this paper, you can download it from here: http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf
What exactly does the Kalman filter do? Let's take a look at the explanation on the wiki:
A typical example of Kalman filtering is to predict the coordinates and speed of the object's position from a set of finite, noise-containing, observation sequences of the object's position (which may be biased). It can be found in many engineering applications (such as radar, computer vision). At the same time, Kalman filtering is also an important subject in control theory and control system engineering. For example, for radar, people are interested in its ability to track targets. However, the measured values of the target's position, velocity, and acceleration are often noisy at all times. Kalman filter uses the dynamic information of the target to try to remove the influence of noise and obtain a good estimate of the target position. This estimation can be the estimation of the current target position (filtering), the estimation of the future position (prediction), or the estimation of the past position (interpolation or smoothing).
Stanley Schmidt (Stanley Schmidt) realized the Kalman filter for the first time. When Kalman visited the NASA Ames Research Center, he found that his method was very useful for solving the orbit prediction of the Apollo project. Later, the navigation computer of the Apollo spacecraft used this filter. Papers on this filter were published by Swerling (1958), Kalman (1960) and Kalman and Bucy (1961).
At present, there are many different implementations of Kalman filter. The form that Kalman originally proposed is now generally called simple Kalman filter. In addition, there are Schmidt extended filters, information filters, and many variants of square root filters developed by Bierman and Thornton. Perhaps the most common Kalman filter is a phase-locked loop, which is widespread in radios, computers, and almost any video or communication equipment.
In simple terms, the Kalman filter is an "optimal recursive data processing algorithm". For solving a large part of the problem, he is the best, the most efficient and even the most useful. It 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 head and face recognition, image segmentation, image edge detection and so on.
2 . Introduction to the Kalman Filter (Introduction to the Kalman Filter)
In order to make it easier to understand the Kalman filter, we first apply a vivid description method to explain, and then we combine its core 5 formulas for further explanation and exploration. Combined with modern computers, Kalman's program is actually quite simple, as long as you understand his five formulas.
Before introducing his 5 formulas, let us make an intuitive explanation based on the following example.
Suppose the object we want to study is the temperature of a room. According to your experience, the temperature of this room is constant, that is, the temperature in the next minute is equal to the temperature in the current minute (assuming we use one minute as the unit of time). Assuming that you are not 100% confident in your experience, there may be a few degrees of deviation up and down. We regard these deviations as White Gaussian Noise (White Gaussian Noise) , that is, these deviations have nothing to do with the time before and after and conform to Gaussian Distribution. In addition, we put a thermometer in the room, but this thermometer is not accurate, and the measured value will deviate from the actual value. We also regard these deviations as Gaussian white noise.
Well, now for a certain minute we have two temperature values related to the room: your predicted value based on experience (predicted value of the system) and the value of the thermometer (measured value). Next, we will use these two values in combination with their respective noises to estimate the actual temperature of the room.
Suppose we want to estimate the actual temperature at time k. First, you have to predict the temperature at time k based on the temperature value at time k-1. Because you believe that the temperature is constant, you will get that the predicted temperature at time k is the same as at time k-1, assuming it is 23 degrees, and the deviation of the Gaussian noise of this value is 5 degrees ( 5 is obtained like this: If the deviation of the optimal temperature value estimated at time k-1 is 3, and your uncertainty of your prediction is 4 degrees, they are squared and then squared, which is 5 ) (5 is the sum of the error covariances of the previous time The process noise Q predicts the new error P k- ) . Then, you get the temperature value at time k from the thermometer, assuming it is 25 degrees, and the deviation of the value is 4 degrees.
Since we use two temperature values to estimate the actual temperature at time k, they are 23 degrees and 25 degrees . What is the actual temperature? Do you believe in yourself or the thermometer? We can use their covariance to judge who we believe more. Because Kg^2=5^2/(5^2+4^2), so Kg=0.78 ( Kg is the Kalman gain ), we can estimate that the actual temperature value at time k is: 23+0.78*(25- 23)=24.56 degrees. It can be seen that because the covariance of the thermometer is relatively small (I trust the thermometer), the estimated optimal temperature value is biased toward the value of the thermometer.
Now that we have obtained the optimal temperature value at time k, the next step is to enter time k+1 to perform a new optimal estimation. So far, it seems that no autoregressive things have appeared. By the way, before entering time k+1, we have to calculate the deviation of the optimal value (24.56 degrees) at time k. The algorithm is as follows: ((1-Kg)*5^2)^0.5=2.35. Here 5 is the deviation of the 23 degree temperature value you predicted at time k above, and 2.35 is the deviation of the optimal temperature value estimated at time k after entering k+1 (corresponding to 3 above).
In this way, the Kalman filter keeps recursing the covariance to estimate the optimal temperature value. He runs very fast, and it only retains the covariance of the previous moment. The Kg above is Kalman Gain. He can change his own value at different moments , isn't it amazing!
Now let’s get back to business and discuss Kalman on a real engineering system.
3 . The Kalman Filter Algorithm
In this part, we will describe the Kalman filter derived from Dr Kalman. The following description will involve some basic conceptual knowledge, including probability (Probability), random variable (Random Variable), Gaussian or normal distribution (Gaussian Distribution) and State-space Model, etc. But for the detailed proof of the Kalman filter, it cannot be described one by one here.
First of all, we must first introduce a discrete control process system. The system can be described by a Linear Stochastic Difference equation, which we illustrate with the following PPT screenshot:
In the above two equations, x(k) is the state of the system at time k, and u(k) is the control quantity of the system at time k. A and B are system parameters. For a multi-model system, they are matrices. y(k) is the measured value at time k, H is the parameter of the measurement system, and for the multi-measurement system, H is the matrix. q(k) and r(k) represent process and measurement noise, respectively. They are assumed to be White Gaussian Noise (White Gaussian Noise), and their covariance is Q and R respectively (here we assume that they do not change with system state changes).
For satisfying the above conditions (linear stochastic differential system, process and measurement are Gaussian white noise), Kalman filter is the optimal information processor. First, the process of the KF algorithm and five core update equations are given as follows:
KF algorithm
The five update equations are:
It is inconvenient to write the formula, so I wrote it as a PDF and made a screenshot and stuck it below. The following is an explanation of the Kalman algorithm with the above example and the five core formulas:
In this way, the algorithm can continue to perform autoregressive operations.
Please see the second article 30 minutes to understand Kalman filter_2 (the second article contains the original code compiled by arduino and stm32 Kalman filter)