This article is written after reading http://blog.csdn.net/adamshan/article/details/78265754 and adding my own understanding. If there is any infringement, please contact me to delete it. If there is something wrong, please Do not hesitate to enlighten me!
Since the Kalman filter can only satisfy the linear model, when encountering a nonlinear model, the Gaussian distribution no longer satisfies the superposition, so can we change the nonlinear to linear?
Good idea, we can use Taylor expansion to expand the original nonlinear state transition formula into first-order or second-order Taylor expansion, so that we can change the original nonlinear into linear, and then apply the original Kalman filter conclusion to do it .
Here we provide a new model: the constant turn rate and velocity model (Constant Turn Rate and Velocity, CTRV) as the nonlinear model of our analysis this time, this model is also very famous, and we will learn more about it later
{
- Constant Velocity Model (Constant Velocity, CV)
- Constant Acceleration (CA)
- Constant Turn Rate and Velocity (CTRV)
- Constant Turn Rate and Acceleration (CTRA)
} These are some sports models, and I will learn about them one by one when I have time later.
In this CTRV model:
Target state quantity:
The transfer equation is:
As w tends to 0, we have new state transition equations:
Obviously, in the above transition equation, it is non-linear. At this time, we cannot directly predict and analyze the state transition matrix as before. At this time, we can get the prediction function:
Among them, g ( ) g() g() represents the state transition function of the CTRV motion model, and u uu represents the processing noise. In order to solve the problem under the nonlinear system, we introduce the Extended Kalman Filter (EKF)
Extended Kalman Filter
The Extended Kalman Filter uses a linear transformation to approximate a nonlinear linear transformation. Specifically, EKF uses a first-order Taylor expansion for linearization:
For this state transition equation, we can find his Jacobian matrix, (a matrix composed of the first-order partial derivatives of each dependent variable with respect to each independent variable in the multivariate function)
The specific Jacobian matrix can be found in the top blog
After obtaining the Jacobian matrix, we can rewrite the original KF formula into the following formula. My understanding of this formula is this: the first state transition formula is easy to understand, which is to bring Xk from the previous state into the state The transition equation finds the next state, but why is the next covariance matrix multiplied by J? It is because we expand the original nonlinear state transition equation Taylor, as follows:
Here u can be regarded as the previous state, which is equal to the Taylor expansion in the previous state. For each state in X after such conversion, we have to bring it into the first-order formula above, which is determined by the properties of variance and covariance In other words, for D(aX+b)=aXa.T
So we get the following formula, plus the uncertainty Q caused by the noise
The same is true for the matrix transformation in the subsequent measurement
