references:
【１】http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
【２】https://blog.csdn.net/u010720661/article/details/63253509
【３】A geometric interpretation of the covariance matrix
【４】Video explanation of Kalman filter from theory to practice-super detailed
Table of contents
1．Introduction to Kalman Filter
3.1 The simplest data fusion—mean estimation
3.2 Classic Kalman filter data fusion
【１】How a Kalman filter sees your problem ？
【２】Describing the problem with matrices
【３】Refining the estimate with measurements
1．Introduction to Kalman Filter
Kalman filtering refers to predicting and correcting the state of the system in a continuously changing dynamic system containing uncertainty information such as noise . Simply put, the Kalman filter predicts the state of the current moment based on the state of the previous moment, weights the predicted state with the measured value of the current moment, and the weighted result is considered to be the current actual state, not just the current state. Trust the current measurements.
The uncertainty here is reflected in:
1) There is no mathematical model that can perfectly model a certain system.
2) The perturbations and disturbances suffered by the system are uncontrollable and difficult to mathematically model.
3) Any measurement sensor has errors
The Kalman filter only needs to store the parameters of the previous state and does not need to retain other historical data. Therefore, it has the advantages of small memory usage and fast calculation speed , so it is very suitable for solving real-time problems and embedded systems. For example, in the field of autonomous driving, when the car is traveling, we will obtain various sensor data in the self-car coordinate system at a certain moment. These data include: 1) The position, speed, and
attitude of obstacles (LiDar, Radar, vision) ;
2) The visual perception results of lane lines and curve equations, types and lengths;
3) The GPS coordinates of the own vehicle, etc.
The combination of these signals represents the current environmental information of the unmanned vehicle. Due to the characteristics of the sensor itself, any measurement results are subject to error. Taking obstacle detection as an example, if the measurement results of the sensor are used directly, the obstacle measurement results may change suddenly when the vehicle is bumpy or affected by other external conditions, which is unacceptable for the perception of unmanned vehicles. Therefore, it is necessary to track based on the sensor measurement results to ensure that the position, speed and other information of obstacles will not change suddenly. In the field of unmanned vehicles, the Kalman filter is not only used in obstacle tracking, but also in lane tracking, obstacle prediction and positioning.
2. kinematic model
Single motion model: These linear motion models assume that the target moves in a straight line and do not consider the turning of the object.
● Constant Velocity (CV) model
● Constant Acceleration (CA) model
Quadratic motion model: usually with angular velocity, that is, the vehicle is moving in an arc
● Constant Turn Rate and Velocity (CTRV) model: moves at a fixed turning rate and constant speed, that is, uniform circular motion
● Constant Turn Rate and Acceleration model (Constant Turn Rate and Acceleration, CTRA)
Compared with CTRV, the rotation speed remains unchanged, which means that the angles rotated in the same time are the same, and the radial direction changes from uniform speed to uniform acceleration. CTRA is mostly used in airborne tracking systems. Most of these secondary motion models assume that the speed v and the yaw angular velocity ω have no relationship. Therefore, in this type of motion model, due to the perturbation of the yaw angular velocity ω measurement, even if the vehicle is not moving, the angular velocity under the motion model will change very slightly.
In order to solve this problem, the relationship between speed v and yaw angular velocity ω can be established by setting the steering angle Φ to be constant, that is, controlling the tire angle through the steering wheel on the car, which leads to the constant steering angle and speed model (Constant Steering Angle and Velocity (CSAV), in addition, the velocity can be assumed to change linearly, which leads to the constant curvature and acceleration model (Constant Curvature and Acceleration, CCA).
3．Classical Kalman filter
3.1 The simplest data fusion—mean estimation
3.2 Classic Kalman filter data fusion
Using all available information to get a better estimation result is the essence of Kalman filtering.
【１】How a Kalman filter sees your problem ？
We use a simple state, assuming only position and velocity:
We don't know the actual position and velocity, there are a range of possible position and velocity combinations, but some of them are more likely:
The above picture is a projection picture of a two-dimensional Gaussian distribution. Kalman filter assumes that the two variables (position and velocity) are randomly Gaussian distributed. Each variable has a mean μ (the center of the random distribution) and a variance σ2 (representing uncertainty). The above figure shows that position and velocity are not related. The concept of correlation is discussed in probability theory. The general meaning is that one variable will not tell you information about another variable.
The following example is more realistic: position and velocity are related. The possible observations at a given position depend on the velocity:
This situation is used to estimate the next state using the previous old state. The greater the speed, the farther the object moves. This relationship is very important when tracking positioning because it provides more information: one measurement tells us the possible range of other measurements. This is the goal of the Kalman filter, we want to get as much information as possible from uncertain measurements.
This relationship is given by the covariance matrix. Simply put, each element Σij of the covariance matrix is the correlation degree between the i-th state variable and the j-th state variable. (So the covariance matrix is symmetric, exchanging i and j has no effect), the covariance matrix is usually represented by "Σ", so its elements are called "Σij".
【２】Describing the problem with matrices
Use matrix Fk to represent this prediction step:
●External influence
We have not used all the information. Some changes in the external world may not have anything to do with the state, but they will affect the system. For example, when the conductor pushes the air valve, the speed of the car will change. Similarly, our robot will send instructions to the wheels to move or stop. If we know more information about the real world, we can write this into a vector called uk and add it to our prediction as a correction .
For example, we know that acceleration a represents a control command, from the basic kinematics formula:
●External uncertainty
If these state quantities change based on the properties of the system itself or known external control effects, there will be no problem
. But what if there are unknown interferences? For example, suppose we track a quadcopter, which may be
disturbed , if we track a wheeled robot, the wheels may slip, or a slight slope on the road may cause it to slow down. In this case, we
cannot continue to track these states, and if these external disturbances are not taken into account, our predictions will be biased.
After each prediction, we can add some new uncertainty to model this uncertainty with the "outside world" (i.e. disturbances we are not tracking
) :
Note: Before increasing the uncertainty, the original point moved to the next moment is somewhere corresponding to the calculation based on the equation, but after
increasing uncertainty, it may be somewhere in the green frame of the new area. This will produce a new Gaussian patch, with new covariance
(same expectation):
It can be seen from the above:
【３】Refining the estimate with measurements
The system has some sensors that indirectly provide some status information. In other words, the sensor gives a series of readings in a certain state. For example, the position signal measured by lidar is the distance in the x direction and y direction in the Cartesian coordinate system, while the millimeter wave radar measures the position and angle information in polar coordinates.
The measurement unit and range of the sensor may be different from the system state. In this case, we need to use the matrix H k for scale conversion. This H k is called the measurement matrix (Measurement Matrix).
Kalman filtering is suitable for processing sensor noise. In other words, the sensor will be somewhat unreliable, and the sensor readings for each state in the original estimate will be a range value. From each reading we observe, we may guess that the system is in a specific state. However, because of uncertainty, the probability of certain states in the observed data will be relatively high. (Seeing may not be believing, some data are on the high side)
Call the covariance of this uncertainty (sensor noise) R k , and the distribution of expected and observed readings to be the same is called zk . Now, we get two Gaussian spots: one represents the system state prediction; one represents the sensor measurement.
The next step is to reconcile the predictions with the sensor measurements. So which one is closer to the true state? If we have two probability distributions and want to know the probability that both are true (overlap), we just need to multiply them (probability theory). The overlapping part is the most likely part, which is based on the existing new best estimate. Looks like another gaussian blob.
What you need to know is that the product of two one-dimensional Gaussian distributions is a new Gaussian distribution , the formula is as follows:
For the detailed derivation process, please refer to (video): Kalman gain super detailed derivation
【４】 Putting it all together、
The Kalman filter can accurately model any linear system . For nonlinear systems, you can use the extended Kalman filter ,
which is to linearize the prediction and measurement of the expected value. Please see "Kalman Filter: Extended Kalman Filter" .