Analysis of the Basic Principles of Kalman Filter
foreword
卡尔曼滤波是一种实时性高，滤波效果好并且计算量小的一种滤波算法，因内存占用不大的适合嵌入式使用而广受欢迎。相较于常见的经典滤波方式卡尔曼滤波使用上简单方便，但涉及到概率分布模型以及迭代存在一定的入门门槛。
本文从卡尔曼滤波基本原理入手，通过讲解卡尔曼基本迭代过程及一阶多阶例程来实现对卡尔曼基本原理的剖析。
1. Introduction to Kalman filter
In the early 1960s, R. E. Kalman and R. S. Bucy published an important paper "New Achievements in Linear Filtering and Prediction Theory", which proposed a new linear filtering and prediction method. In theory, it is called the Kalman filter. The characteristic is to process the noisy input and observation signal on the basis of linear state space representation, and obtain the system state or real signal.
This theory is expressed in the time domain, and the basic concept is: on the basis of the state space representation of the linear system, the optimal estimation of the system state is obtained from the output and input observation data. The system state mentioned here is a collection of the minimum parameters that summarize all past inputs and disturbances of the system on the system. Knowing the state of the system can determine the entire behavior of the system together with future inputs and system disturbances.
Since the advent of the Kalman filter theory, it has been applied in many departments such as communication systems, power systems, aerospace, environmental pollution control, industrial control, and radar signal processing, and has achieved many successful application results. For example, in image processing, Kalman filter is used to restore blurred images caused by some noise [1] .
2. Basic principles
Kalman filtering can be considered as a special case of Bayesian filtering, which essentially obtains posterior probability by superimposing likelihood probability and prior probability according to Bayesian formula. Understanding from the most essential principles requires a lot of knowledge reserves in probability theory, but it is relatively simple to understand from the form of use.
Formally understood is the weight superposition of the measured value and the estimated value. In order to facilitate the superposition of the measured value and the estimated value, the Gaussian distribution model is used to describe the expected value and error, and then the estimated value and the measured value are superimposed and output by using the formula properties of the Gaussian distribution model. , to calculate the error range of the optimal value. The filtering process of the next cycle is consistent with the current process, and it only needs to be iterated continuously.
2.1 Weight configuration of measured values and estimated values
The basic understanding of KMF should start with the optimal value formula of the final output:
In the above formula, x- represents the optimal output value, x^ is the estimated value, z is the measured value, and kg represents the Kalman gain.
It can be seen from the formula that the Kalman filter is consistent with the first-order low-pass filter and complementary filter, and depends on the weight coefficient kg to affect the final output value. The source of the predicted value depends on the control system model. For example, if the current temperature is 30°, heating with a power of 10° per second can predict that the temperature will be 50° after 2 seconds (ignoring the effects of heat dissipation, etc.), and the error is 5 degrees. Of course, in the case of simple first-order filtering, the control model can be simplified, and the predicted value after 1s is always the actual optimal output value before. The measured value is obtained from the sensor, and the size of the error is obtained from the sensor manual.
So we understand:
The current output optimal value = estimated value + Kalman gain (weight) * ( measured value - estimated value )
is different from the first-order low-pass filter. The advantage of Kalman filter is that the weight is not a fixed value, and it will continue to iterate change by calculation. Therefore, in order to clarify the Kalman filter algorithm, it is necessary to understand how the Kalman gain coefficient comes from. The Kalman gain coefficient is obtained thanks to the Gaussian distribution mathematical model.
2.2 Description of Gaussian distribution mathematical model
From the above we know that the optimal output value is the superposition of the estimated value and the measured value , but the estimated value and the measured value are not an accurate value, and there is a certain error. For example, the sensor measures the temperature, the measured value is 30 degrees, and the error is 2° ; The estimated value should also be the accuracy of the model, there are process errors in the calculation process, as well as external interference errors. The Gaussian expected-variance form and this value-error form are well suited to describe this model.
Therefore, Kalman uses the Gaussian probability distribution model to describe each component value and its error in a concrete functional way, and indirectly introduces the amount of the error value.
The Gaussian distribution is also called the normal distribution, and its probability density function is as follows:
e is the natural base; σ ^2 is the variance, corresponding to the error; μ is the expectation, corresponding to the estimated value/measured value
The graph of the function is as follows
The probability of a value in this interval can be obtained by integrating a certain interval. The area of the entire integral interval is 1. The larger the variance value of σ^2, the more "fat and short" the function graph is, whereas it is "tall and thin". The μ expectation determines the offset of the function graph on the X-axis.
Suppose the measured value is 30, the variance is 3, the estimated value is 32, and the variance is 4, as shown in the figure:
The red lines represent measured values and the blue lines represent estimated values.
It can be seen from the above figure that the two images have overlapping parts, and the confidence interval of the overlapping part indicates that the value of this part satisfies both the probability distribution of the estimated value and the probability distribution of the measured value, so we hope that the value distribution after the superposition of the estimated value and the measured value within this area. The superposition property of the Gaussian distribution can be used to achieve this effect.
The probability density formula of the Gaussian distribution is an exponential form, and the product of the probability density functions of two Gaussian distributions still satisfies the Gaussian distribution. The superposition process is deduced as follows:
1. Assume that the Gaussian distribution probability density functions of the estimated value and the measured value are: 
2. Multiply according to the properties of the Gaussian formula:
this part is the core part of Kalman filtering, and the superposition principle formula is Bayesian formula: posterior probability = kx likelihood probability x prior estimate probability. Correspondingly, the optimal output probability density function = kx measured value probability density function x estimated value probability density function.
3. Simplify and construct a new form of Gaussian formula
Sg is called the compression coefficient, which determines the size of the total probability area.
3. Correction of the Sg coefficient.
It can be seen from the above that the formula superimposed after the product does not fully satisfy all the properties of the Gaussian distribution, because the constant term of Sg will make the total area ( The total probability) is enlarged or reduced, so in order to facilitate the iteration of the Gaussian distribution, let Sg=1, without changing the actual optimal output expected value u, and the error, then the superimposed value still satisfies all the properties of the Gaussian distribution.
Continue to the previous measurement value of 30, the variance is 3, the estimated value is 32, the variance is 4, and the superposition does not change the size of Sg:
Among them, the red line represents the measured value, the blue line represents the estimated value, and the green line represents the probability density function superposition.
Let the Sg coefficient be 1, and a new distribution function graph can be obtained:
The red line represents the measured value, the blue line represents the estimated value, and the green line represents the corrected probability density function.
6. Transform the superimposed variance σ ^2 and expected μ, and extract the Kalman gain coefficient Kg.
So far, the Gaussian distribution description and superposition of the estimated value and measured value have been completed, and the optimal estimated value can be obtained according to the superimposed expected value u. The superimposed variance is used for the next estimate variance calculation.
After a single superposition is completed, the follow-up is the same iterative process
2.3 Iteration based on Gaussian distribution model
The essence of Kalman filtering is the iterative process of the Gaussian distribution model. Here is an iterative combing:
it can be divided into three parts:
- Establishment of Gaussian distribution model of estimated value - prediction
The establishment of Gaussian distribution model requires two parameters: expectation μ (estimated value) and variance σ^2.
The estimated value is related to the control model. For example, if the current temperature is 30°, heating at a power of 10° per second can predict that the temperature will be 50° after 2 seconds (ignoring the effects of heat dissipation). Generally, the first-order Kalman will compare the estimated value with the above The optimal output value at one time remains consistent, that is, 30°.
The estimated value variance σ 2 is related to the control model and external disturbance noise. Based on the last optimal output error, the control model will transmit the error, so there is process noise. If there is external interference, the error will increase, so the new variance σ 2 = last optimal output variance *a+q; Generally, the first-order Kalman filter will simplify to make a=1, only consider the noise q that exists in the interference, and it is also one of the parameters that Kalman filter needs to adjust.
- Establishment of Gaussian distribution model of measured value – observation
The expected value μ of the Gaussian distribution model of measured value is obtained from the real-time measured value of the sensor, and the variance σ 2 is obtained from the sensor manual. If it cannot be found, a set of data can be measured and calculated according to the variance formula. The variance is general Sure it won't change.
- The establishment of the superposition Gaussian distribution model - update
The superposition process is to first calculate the Kalman gain, and then calculate the expected value (optimal output value) of the Gaussian distribution after the superposition, and at the same time calculate the variance of the Gaussian distribution after the superposition.
Using mathematical formulas to describe the whole process is 6 formulas:
estimated value and variance calculation (prediction):
measured value and variance acquisition (observation):
optimal output value and variance calculation (update):
first calculate the Kalman gain
formula 3, 4 needs Obtained from the sensor, the remaining formulas 1, 2, 4, 5, 6, and 7 are the most common Kalman filter iterative calculation formulas.
3. First-order Kalman filter
The first-order Kalman filter is the easiest to understand and the easiest breakthrough. After explaining the basic principles, it is easier to understand the whole process with an example.
3.1 Temperature example model
We want to study the temperature of a room and estimate the actual temperature value at time k.
- Estimated value Gaussian distribution (prediction)
First, you need to predict the temperature at time k (experienced temperature at time K) based on the temperature value at time k-1. Because you believe that the temperature is constant, you will get the predicted value of the temperature at time k is the same as that at time k-1, assuming it is 23 degrees (equation (1)), and the variance of the Gaussian noise of the predicted value is 5( 5 is obtained in this way: if the variance of the optimal temperature value estimated at time k-1 is 3, there are some heat dissipation interference factors 2, and the sum is 5 (formula (2)) ). - Gaussian distribution of measured values (observations)
Then, you get the temperature value at time k from the thermometer, let's say 25 degrees, and the sample variance of this value is 4 degrees. - Optimal Output Gaussian Distribution (Updated)
Now, we have two temperature values for estimating the actual temperature of the room at time K: an estimated value of 23 degrees and a measured value of 25 degrees. What is the actual temperature? Do you believe in yourself or the thermometer? Who do you trust more? We need to use their variance to judge. H=5 5/(5 5+4 4)=0.61 (formula (5)), so we can estimate the optimal temperature value at time K: 23+ 0.61 (25-23)=24.22 degrees (formula (6 )) The optimal temperature at time K is obtained, because the variance calculation of the estimated temperature value at time K+1 needs to obtain the variance of the optimal temperature (24.22) at time K, so the variance of the optimal temperature also needs to be updated, (1- H)*5=1.95 (formula (7)).
In this way, the Kalman filter continuously recurses the error to estimate the optimal temperature value, which runs fast and only retains the covariance of the previous moment.
3.2 Simplified description of the first-order Kalman filter
The first-order Kalman filter can be simplified a lot, as shown in the following points:
- Simplify the control model, the current estimated value is the same as the last optimal output value, and the control effect is skipped.
- Simplify the transfer of control to the error of the estimated value. Generally, the variance of the estimated value is directly superimposed on the basis of the variance of the last optimal output value with external interference noise. In fact, the external interference noise can also be omitted (the specific effect has not been compared).
3.3 First order Kalman filter code
Two first-order versions are provided, one is general-purpose, the other omits the noise q, and fuses the iterative energy fusion process formula, but it is not tested for reference only.
3.3.1 Basic Universal Version
.h file
#ifndef __PUB_KMF
#define __PUB_KMF
typedef struct
{
float p;//最优输出方差
float q;//噪声
float r;//传感器方差
float Kg;//卡尔曼增益
float x;//最优输出
}ST_KMF;
class pub_kmf
{
/*---------DATA-----------*/
public:
ST_KMF st_kmf;
/*------------FUNCTION----------*/
public:
pub_kmf(){
};
~pub_kmf(){
};
void InitValue(void);
float KalmanFilter(ST_KMF *st_kmf,float sensorVal);
};
#endif
.cpp file
#include"pub_KMF.h"
pub_kmf st_kmf;
void pub_kmf::InitValue(void)
{
st_kmf.q =0.1;
st_kmf.r =0.2;
st_kmf.p =0;
st_kmf.Kg =0;
st_kmf.x =0;
}
float pub_kmf::KalmanFilter(ST_KMF *st_kmf,float sensorVal)
{
//计算估计值
st_kmf->x = 1*st_kmf->x
//计算估计值方差
st_kmf->p = st_kmf->p +st_kmf->q;
//计算卡尔曼增益
st_kmf->Kg = (st_kmf->p*st_kmf->p)/(st_kmf->p*st_kmf->p+st_kmf->r*st_kmf->r);
//计算最优输出
st_kmf->x = st_kmf->x +st_kmf->Kg*(sensorVal-st_kmf->x);
//计算最优输出方差
st_kmf->p = (1-st_kmf->Kg)*st_kmf->p;
}
3.3.2 Minimal version
.h file
#ifndef __PUB_KMF
#define __PUB_KMF
typedef struct
{
float p;//最优输出方差
//float q;//噪声
float r;//传感器方差
//float Kg;//卡尔曼增益
float x;//最优输出
}ST_KMF;
void InitValue(void);
float KalmanFilter(ST_KMF *st_kmf,float sensorVal);
extern ST_KMF st_kmf;
#endif
.c file
#include"pub_KMF.h"
ST_KMF st_kmf;
void InitValue(void)
{
st_kmf.r =0.2;
st_kmf.p =0.1;
//st_kmf.Kg =0;
st_kmf.x =0;
}
float KalmanFilter(ST_KMF *st_kmf,float sensorVal)
{
//计算估计值
//st_kmf->p = st_kmf->p
//计算估计值方差
//st_kmf->p = st_kmf->p;
//计算卡尔曼增益
//st_kmf->Kg = (st_kmf->p*st_kmf->p)/(st_kmf->p*st_kmf->p+st_kmf->r*st_kmf->r);
//计算最优输出
st_kmf->x = st_kmf->x +((st_kmf->p*st_kmf->p)/(st_kmf->p*st_kmf->p+st_kmf->r*st_kmf->r))*(sensorVal-st_kmf->x);
//计算最优输出方差
st_kmf->p = ((st_kmf->r*st_kmf->r)/(st_kmf->p*st_kmf->p+st_kmf->r*st_kmf->r))*st_kmf->p;
}
4. Multi-order Kalman filter
The multi-order Kalman filter is consistent with the first-order principle, the difference is:
- A control model must be added, and even a simple control model must reflect the relationship between state parameters, such as the position and speed of the robot during motion. Otherwise, the essence is still first-order, and the multi-order model is very suitable for the fusion of multi-sensor data.
- The variance becomes covariance, which not only reflects the error but also shows the correlation information between the transition parameters.
- Convert from ordinary linear equation calculation to matrix operation.
- Easy to fuse multi-sensor data
This section explains on the basis of the first-order principle, focusing on the multi-order calculation process, taking the second-order robot position and velocity model as an example of operation, and the basic principles of more-order models are consistent with this.
Reference example
A robot is moving on flat ground. In order to realize position and speed control, a position and speed sensor is added to the robot for feedback of position and speed.
Therefore, x can be used to represent the current state of the robot, including the position and velocity components of the robot, which is called a state vector. The expression of the state vector is as follows:
Create a kinematic model of the car
The matrix represents the above formula:
F is the state transition matrix. B is the control matrix.
Of course, if it is more complete, there will be noise.
The x output at this time is the prior estimate, that is, the estimated value.
The estimated value corresponds to the variance of the estimated value, but the multi-state quantities are not independent of each other, so the calculation of the variance between different quantities is no longer the degree of dispersion of a single quantity, but the correlation between two quantities. Unity is represented by covariance.
The covariance of the robot position velocity model is then
The covariance update of the prior estimate is given by the following formula:
The addition of the state transition matrix indicates that the error will follow the state transition, and Q represents the estimated noise.
At this time, the update of the estimated value model has been completed. The following is the measured value model, also known as the posterior estimation model.
Note that the output value of the sensor is not necessarily the element in the state vector we created, and sometimes a simple conversion is required. Even if it is, it is possible that the units do not correspond, so a conversion is required. This transformation is the matrix, which is also called the mapping matrix from state space to observation space in some literatures. For example, the sensor can only measure the position and velocity, but the calculation requires adding an acceleration to the state vector, and the acceleration value can be calculated from the change in velocity per unit time, so the measurement vector is a 2x1 matrix, the state vector is a 3x1 matrix, and a 3x3 is needed in the middle the transformation matrix. Of course, the measurement noise Vk can also be added to the measurement quantity. Corresponding measurement covariance [1] .
After the measurement model is updated, the next step is to superimpose the model. According to the first-order Kalman filter superposition formula, first calculate the Kalman gain coefficient, and then calculate the optimal output and variance to derive the multi-order Kalman gain coefficient matrix, optimal state vector and corresponding covariance: first-order second-
order
The process and principle are consistent with the first order, and the operation of covariance and matrix will make the operation look complicated, but in fact it provides great convenience for the processing of multi-sensor and multi-state parameters.
5. Parameter adjustment
For parameter adjustment, refer to this article.
Let’s extract the part first for ease of use:
Q value is the process noise, the smaller the system, the easier it is to converge, and the higher our confidence in the value predicted by the model; but it is easy to diverge if it is too small. If Q is zero, then We only believe in the predicted value; the larger the Q value, the lower our trust in the prediction, and the higher the trust in the measured value; if the Q value is infinite, then we only trust the measured value; the R value is the measurement noise, too Too small or too big is not necessarily suitable. If R is too large, the Kalman filter response will slow down, because its confidence in the newly measured value will decrease; the smaller the value, the faster the system will converge, but if it is too small, it will easily oscillate; during the test, you can keep the gyroscope still and record a period The output data of the gyroscope within the time period is approximately normally distributed. According to the 3σ principle, (3σ) 2 of the normal distribution is taken as the initialization value of R.
When testing, you can first adjust Q from small to large, and adjust R from large to small; first fix one value and adjust another value to see the convergence speed and waveform output.
There is also a key value P in the system, which is the initial value of the error covariance, which indicates our confidence in the current forecast state. The smaller it is, the more we believe in the current forecast state; its value determines the initial convergence speed, and it is generally set at the beginning. A smaller value allows for faster convergence. With the iteration of Kalman filtering, the value of P will continue to change. When the system enters a steady state, the value of P will converge to a minimum estimated variance matrix. At this time, the Kalman gain is also optimal, so this value only affects initial convergence rate.
6. Kalman filter extension
Although the Kalman filter is a filter with strong real-time performance, it is only for linear systems (the state transition is a linear relationship, that is, the current estimated value Xk is proportional to the last optimal Xk-1), and the error estimation is only based on the specified sensor Error and calculation noise error are not a kind of error that is statistically calculated for the actual process and records the last output value. Therefore, the algorithm of extended Kalman filter appeared. The difference is that the estimation and measurement become nonlinear function iteration form. In order to facilitate the processing of nonlinear part, the form of Taylor first-order expansion is used to indirectly transform it into linear.
Summarize
I roughly sorted out the iteration and principle of the first-order Kalman filter, and there are few explanations for high-order and extension. If you are interested, you can refer to the link in the reference material.
The overall process is divided into three parts: prediction, observation, and update. After understanding these three processes, the design program parameter modification can basically be used.