Xu finished the main part of this course, section 3, section 2.2, section 2.3, and Mao Bo finished the arrangement, revision and typesetting of the full text.
Kalman filter and its series of optimization and improvement algorithms are the most commonly used and effective methods to solve the problem of motion state estimation. When Rudolf Emil Kalman visited the Ames Research Center of NASA, he found that his method was suitable for solving the orbit prediction problem of Apollo program. The navigation computer of Apollo spacecraft uses this filter to predict the trajectory.
Kalman filter is especially suitable for dynamic systems. This method has extremely low memory requirements, fast operation speed and good calculation accuracy, which makes it very suitable for solving real-time problems and applying it to embedded systems. In other words, Kalman filter is naturally suitable for solving the track calculation problem of warship command and control system. In the following content, we will gradually understand these excellent characteristics of Kalman filter.
However, now let's get rid of the complicated problem of ship track calculation and understand the idea, process and algorithm of this filtering algorithm from a more familiar and simpler problem.
Suppose there is an unmanned vehicle WALL-E, and there are two ways to guide it from point A to point B (Figure 1):
Obviously, both methods have certain errors. If a certain method is used for positioning alone, WALL-E will not be able to reach point B under the influence of error. Therefore, it is necessary to combine the two methods to get more accurate results, which is the problem to be solved by Kalman filter.
How does Kalman filter solve our problem? Before discussing this problem, we abstract the problem and describe our problem in mathematical language.
We use vector to describe the motion state of Wall-E. This column vector includes two components: position vector and velocity vector. As for the problem of WALL-E, we don't know the exact values of position and velocity now, but we know that the motion model of WALL-E satisfies the state equation, and the method of positioning, that is, the method of observing the motion state of WALL-E satisfies the observation equation. Of course, we also know that there are some errors in both methods, so our problem can be transformed into an optimization problem-
In this optimization problem, the objective function is to minimize the prediction (estimation) error and is constrained by the sum of estimation methods. In Kalman filter, our estimation principle (that is, the principle of minimizing estimation error) is minimum variance unbiased estimation [1], which we will explain through the later process analysis.
Before introducing the formula to analyze the Kalman filtering problem, we must also solve a problem-discretizing the continuous linear system, that is, transforming the continuous time domain problem into a time series problem. Of course, at present we only discuss linear systems. For nonlinear systems, we have two methods to solve it: Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF).
Supplementary content-discretization of continuous linear time-varying systems
Let the time-domain state equation of continuous linear time-varying system be
If the sampling period is, sometimes, there is
Make,, then the discretized state equation is
By discretizing the linear system, we can now consider the motion state of WALL-E at every moment. Next, we will use to represent the optimal estimation of the motion state at this moment; The predicted value of the state of time is expressed in time; The optimal estimation value of comprehensive prediction and observation time is expressed as.
As for the problem of estimating the position of WALL-E, assuming that we already know that it is moving in a straight line at a constant speed, and that WALL-E carries a GPS sensor to provide its position information, WALL-E may encounter some situations in the process of moving forward, such as stopping moving forward or being affected by the wind.
Plus what we know is the best estimated state of WALL-E at the last moment, that is, the position and speed at k- 1, but what we need is the best estimated state at the next moment, that is, the position and speed at k moment, we can find that there are two ways to get its state at k moment:
One is to calculate the state of the next second through the WALL-E setup program. For example, if it is set to move in a straight line at a constant speed, the speed of the next second should be constant, and the position is based on the position of the previous second plus the time multiplied by the speed, that is, the distance traveled in one second. However, it is not ideal in real life. The robot will be affected by friction and wind, and of course there may be naughty children blocking his way forward. These factors make Kawara Tsutomu real in K ..
The other is to determine the position of WALL-E through the portable GPS, because GPS measures the real-time state of WALL-E, so it is more accurate. But there are two problems when GPS measures the state at K. First, GPS can only measure the position of WALL-E, but not its speed. Secondly, there will be instrument error when GPS sensor measures, which can only be said to be more accurate and close to the true value.
Then the next question is, how do we get the real state of WALL-E at K?
We call the state value obtained by the first method as the predicted value and the state value obtained by the second method as the measured value. The best estimate of the car is to combine these two parts of information and try to approach the real value of K moment.
Let's think deeply, how do these two parts combine?
At the initial time k- 1, it is the best estimate of WALL-E, which can be anywhere near the estimate. This uncertainty is described by this probability density function. WALL-E is most likely to be close to the average of the distribution. The next time, the uncertainty of estimation increases, which shows that the variance becomes larger. This is because between the time steps k- 1 and k, the WALL-E may be affected by the wind, or its feet may slide forward a little, so it may travel a distance different from the distance predicted by the model.
Another source of information about Kawara Tsutomu's location is measurement. Variance indicates the uncertainty of error measurement, and the real position can also be anywhere near the average value.
The best estimate of WALL-E is to combine these two parts of information and multiply two probability functions to get another Gaussian function. The variance of this estimate is smaller than the previous estimate, and the average value of this probability density function provides us with the best estimate of the position of Wall-E. ..
Next, we will deduce the operation of e.
Settings:
The actual target variable has an expression:
Expression of target variables in mathematical model;
Expressions of measured variables in the actual model:
Expressions of measured variables in mathematical models;
Subtract the actual value and estimated value of the target variable:
Bringing the above equation into the expression of error e, we can get the analytical solution of error e:
It is not difficult to see from the derived results that the error between the estimated value and the actual value changes exponentially with time, when (f-KH)
On the issue of estimating the position of WALL-E, we don't know the exact values of position and velocity, but we can give an estimation interval (Figure 5.a). Kalman filter assumes that all variables are random and conform to Gaussian distribution (normal distribution). Each variable has a mean and a variance (Figure 5.b). Fig. 5c shows that speed and position are related.
If we know the position value of the previous state, we should now predict the position value of the next state. If we are fast, we will go further. On the contrary, if the speed is slow, WALL-E won't go very far. This relationship is very important for tracking system status. It gives us more information: one observation tells us what another observation might look like. This is the purpose of Kalman filter-to extract valuable information from all uncertain information.
According to the knowledge of mathematical statistics, we know that the relationship between two observed values (random variables) can be obtained by a covariance matrix.
Description (Figure 6).
We assume that the distribution of the system state is Gaussian distribution (normal distribution), so at this moment we need two pieces of information: the best estimate and its covariance matrix (as shown in Equation (2)).
Next, we need to predict the current state through the current state. Note that we don't know the exact value of the state, but our prediction function doesn't care. It just predicts the range of all possible values at this moment, and then gets a range of new values at K time. During this process, the position and speed change as follows
We can describe this transformation relationship through the state transition matrix.
Similarly, we update the covariance matrix to
So far, we have considered the case of uniform motion, that is, the system does not control the motion state of WALL-E. Then, if the system controls Wall-E, for example, we can use a vector to describe these extra information and add it to our prediction equation as a correction. If we get the expected acceleration through the issued command, the equation of motion will be updated as follows.
The introduction matrix is expressed as
Here called control matrix and control vector (such as acceleration). Of course, this part can be ignored if there is no external power system.
Let's add one more detail What if our prediction transformation matrix is not 100% accurate? If the state only evolves according to the characteristics of the system itself, there will be no problem. If the influence of all external forces on the system can be calculated very accurately, there will be no problem. But if there are some external forces that we can't predict, such as we are tracking a quadcopter, it will be affected by the wind; Or follow the wheeled robot, the wheels may slip and the protrusions on the ground will slow it down. We can't track these factors, and when these uncertain events happen, the prediction equation will be invalid. Therefore, we model these uncertainties in a unified way and add an uncertain term to the prediction equation.
In this way, every point in the original state can be transformed into a range instead of a certain point (Figure 7.a). It can be described that each point is moved into a Gaussian distribution conforming to the variance (Figure 7.b). In other words, we describe these uncertainties as Gaussian noise with variance of, and express them with. This will produce a new Gaussian distribution with different variance but the same mean (Figure 7.c).
Through the superposition and expansion of, the complete prediction conversion equation is obtained as follows
The new predictive conversion equation only introduces the known system control factors. The new uncertainty can be calculated from the previous uncertainty. At this point, we get a fuzzy estimation range-the range of adoption and description.
Our previous work is still to use the method of motion model to estimate the state of the system. Now, we need to take another method, that is, observation (in this case, GPS positioning), to further correct the estimation of the motion state (Figure 8).
We use matrix to describe the function of observation method, so there are
Adding observation noise, the observation equation is
From the perspective of cybernetics, we define innovation (that is, the error between the observed value and the predicted value) as
Of course, we also know that there will be errors in observation itself. For example, the GPS positioning accuracy in this problem is only 10m. So we use a matrix to describe this uncertainty (figure 10 and figure 1 1.a).
At this point, the covariance of our innovation is
Now we need to combine the possibilities obtained by the two methods (Figure 1 1.b). There are two possibilities for any state: 1. The observed value of the sensor is closer to the real state of the system; 2. The estimated value calculated by the model is closer to the real state of the system. If there are two independent ways to obtain the system state, and we want to know the accurate probability values of both, then we can solve the problem of who is more credible by weighting (Figure 1 1.c).
We now know that the state prediction of the system model and the state observation of the system obey Gaussian distribution. The problem of abstraction is-
According to one of our estimation criteria-minimum variance estimation, this problem can be transformed into an optimization problem.
Find the derivative (difference)
So, like this
When the dimension is higher than one dimension, we use matrix to describe it, and there are
This is called Kalman gain, which is the degree of deviation when we solve which method we trust more.
If we estimate the system state from two independent dimensions, then the prediction based on the system model is
Observations through the sensor are as follows
We get it by combining two methods.
Therefore, the Kalman gain is
Also coming soon (including the project).
The Kalman gain at this time is actually
Let's finally verify the unbiased estimation-
Here we set the real value of this moment to, because
Because of the unbiased transmission from the initial value, it is known that Kalman filter meets the unbiased estimation criterion. Obviously, it is required that the system noise and observation noise are uncorrelated, zero expected white noise, linear system, and the initial state estimation are unbiased. When these conditions cannot be met, the estimation result of Kalman filter is biased.
So far, we have obtained all the elements of the Kalman filter. We can summarize the whole process into three basic assumptions.
Suppose that both sums are zero-mean Gaussian white noise, that is,
Hypothesis two is irrelevant, that is,
Assume that the mean and variance of the initial values of the three systems are known and have no correlation with them.
And five basic equations-state equation prediction.
Equation 2 Covariance prediction
Equation 3 Kalman gain