Kalman Filter (Kalman Filter) concept introduction and detailed formula derivation

Kalman filter

Kalman filter is a high-efficiency recursive filter ( autoregressive filter ) that can estimate the state of a dynamic system from a series of incomplete and noisy measurements . Kalman filtering will consider the joint distribution at each time according to the value of each measurement at different times, and then generate an estimate of the unknown variable, so it will be more accurate than the estimation method based only on a single measurement. Kalman filtering gets its name from Rudolf Kalman, one of the main contributors .

The algorithm of the Kalman filter is a two-step procedure. In the estimation step, the Kalman filter produces an estimate about the current state, which also includes uncertainty. Just observe the next measurement (which must contain some degree of error, including random noise). Estimates are updated through a weighted average , and measurements with higher certainty are weighted more heavily. The algorithm is iterative and can be executed in a real-time control system , requiring only the current input measurements, past calculated values, and its uncertainty matrix, and no other past information is required.

Applications

A typical example of Kalman filtering is to predict the coordinates and velocity of the object's position from a limited set of noisy sequences of observations of the object's position ( possibly biased ) . It can be found in many engineering applications (such as radar , machine vision ). At the same time, Kalman filtering is also an important topic in control theory and control system engineering .

For example, with radar, people are interested in their ability to track targets. But the measured values ​​of the target's position, velocity, and acceleration are often noisy at any time. Kalman filtering uses the dynamic information of the target to try to remove the influence of noise and get a good estimate of the target position . This estimate can be an estimate of the current target position (filtering), an estimate of future positions (prediction), or an estimate of past positions (interpolation or smoothing).

Basic Dynamic System Model

Kalman filtering is built on linear algebra and hidden Markov models (hidden Markov model). Its basic dynamical system can be represented by a Markov chain built on a linear operator disturbed by Gaussian noise (i.e., normally distributed noise) . The state of the system can be represented by a vector whose elements are real numbers . With each increase of the discrete time , this linear operator will act on the current state to generate a new state, and will also introduce some noise, and at the same time, some known controller control information of the system will also be join in. At the same time, another linear operator, corrupted by noise, produces the visible output of these hidden states.

In order to use the Kalman filter to estimate the internal state of the observed process from a series of noisy observation data, the process must be modeled under the framework of the Kalman filter. That is to say, for each step k , define the matrix $F_k$, $H_k$, $Q_k$, $R_k$, sometimes it is also necessary to define $B_k$,as follows.

​ Kalman filter model assumes $k+The\; real\; state\; at\; moment\; 1$ is fromkkThe state at time$k$ evolves and conforms to the state equation:
$$x_k=F_k{x}_{k-1}+B_k{u}_k+w_k$$
in

• $F_k$is acting on $x_{k-1}$The state transition model (matrix or vector) on .
• $B_k$is acting on the controller vector $u_k$The input-control model on .
• $w_k$is the process noise, and it is assumed that its mean is zero, and the covariance matrix is $Q_k$multivariate normal distribution.

​ $w_k\sim N(0,Q_k)$

where $cov\left\{w\right(k)\}=Q\left(k\right)$ represents the covariance matrix.

​ At time k, for real state $x_k$A measurement of $z_k$Satisfy the following formula, that is, the observation equation :
$$z_k=H_k{x}_k+v_k$$
where $H_k$is the observation model, which maps the real state space to the observation space, $v_k$is the observation noise, its mean is zero, and the covariance matrix is $R_k$And obey the normal distribution.
$$v_k\sim N(0,R_k)$$
where$cov\left\{v\right(k)\}=R\left(k\right)$ represents the covariance matrix.

​ Initial state and noise at each moment { x 0 , w 1 , … , wk , v 1 , … , vk } \{x_0,w_1,…,w_k,v_1,…,v_k\}{ x0​,w1​,…,wk​,v1​,…,vk​} are considered to beindependentof each other.

algorithmic logic

Kalman filtering is a recursive estimation, that is, the estimated value of the current state can be calculated as long as the estimated value of the state at the previous moment and the observed value of the current state are known, so there is no need to record historical information of observations or estimates. The Kalman filter is different from most filters in that it is a pure time-domain filter. It does not need to be designed in the frequency domain and then converted to the time domain like low-pass filters and other frequency domain filters. accomplish.

Kalman filtering is mainly divided into two parts: prediction and update . It needs to construct the state equation and observation equation of the system, and the initial state of the system is known. In the prediction phase, the filter uses estimates from the previous state to make an estimate of the current state. In the update phase, the filter optimizes the prediction obtained in the prediction phase with observations of the current state to obtain a new, more accurate estimate.

The state of the Kalman filter is represented by the following two variables:

• ${\hat{X}}_{k|k}=E(X_k|Y_1,Y_2,\dots ,Y_k)$ represents the estimate of the state at time k.
• ${\hat{X}}_{k|k-1}=E(X_{k-1}|Y_1,Y_2,\dots ,Y_{k-1})$ represents the prediction of the state at time k+1 given the state of the past k moments.
• ${\hat{P}}_{k|k}$, the posterior estimation error covariance matrix, which measures the accuracy of the estimated value.

​ Then the prediction and update (Prediction-Correction) process is as follows:
$${\hat{X}}_{k-1|k-1}\stackrel{Prediction}{\to }{\hat{X}}_{k|k-1}\stackrel{Correction}{\to }{\hat{X}}_{k|k}$$

1. Predict ${\hat{X}}_{k-1|k-1}\Rightarrow {\hat{X}}_{k|k-1}$

​ In the prediction step, the state at the current moment is predicted based on the state and control amount at the previous moment. This predicted value is an estimate because it does not take into account the observed values ​​at the current moment. The error covariance matrix of the predicted value is calculated from the error covariance matrix and the system noise covariance matrix at the previous moment.
$$\{\begin{array}{l}{\hat{x}}_{k|k-1}=F_k{\hat{x}}_{k-1}+B_k{u}_k\\ \\ P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k\end{array}$$

2. 更新${\hat{X}}_{n|n-1}\Rightarrow {\hat{X}}_{n|n}$

In the update step, the estimated value of the state at the current moment is calculated according to the observed value and the predicted value at the current moment. This estimate is a more accurate estimate because it already takes into account the observations at the current moment. The error covariance matrix of the state estimate is calculated from the error covariance matrix, observation noise covariance matrix and Kalman gain calculated in the prediction step.
$$\{\begin{array}{l}{K}_k=P_{k|k-1}{H}_k^T(H_k{P}_{k|k-1}{H}_k^T+R_k{)}^{-1}\\ \\ {\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-H_k{\hat{x}}_{k|k-1})\\ \\ P_{k|k}=(I-K_k{H}_k)P_{k|k-1}\end{array}$$
The above five formulas are the core formulas of Kalman filtering. The more concise and easy-to-understand form of the update step is to first calculate the following three quantities:
$$\{\begin{array}{l}{\hat{y}}_k=z_k-H_k{\hat{x}}_{k|k-1}(measurement\; residual)\\ \\ S_k=H_k{P}_{k|k}{H}_k^T+R_k(Measurement\; residual\; covariance)\\ \\ K_k=P_{k|k-1}{H}_k^T{S}_k^{-1}(Optimal\; Kalman\; Gain\end{array}$$
These are then used to update the filter variables:
$$\{\begin{array}{l}{\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{\hat{y}}_k(updated\; state\; estimate)\\ \\ P_{k|k}=(I-K_k{H}_k)P_{k|k-1}(updated\; covariance\; estimate\end{array}$$
Use the above formula to calculate $P_{k|k}$Valid only at optimal Kalman gain. For other gains, the formula is more complicated, please refer to the derivation .

3. Kalman filtering process

Formula Derivation

1. Symbol Definition

State equation
$$x_k=F_k{x}_{k-1}+B_k{u}_k+w_k$$
Among them $u_k$is the input signal, $x_{k-1}$is the state variable, $w_k$Indicates the process noise. where $cov\left\{w\right(k)\}=Q(k)$。

Observation equation
$$z_k=H_k{x}_k+v_k$$
where $z_k$is the observed variable, $w_k$represents the observation noise. where cpv { w ( k ) } = R ( k ) cpv\{w(k)\}=R(k)cpv{ w(k)}=R(k)。

State estimation error
$${\tilde{x}}_{k|k-1}=x_k-{\hat{x}}_{k|k-1}$$
where ${\tilde{x}}_{k|k-1}$Indicates that the estimated $k-1$ moment for$x_k$estimate. At time k-1, any value at time k can only be estimated (predicting future values) .

State estimation equation
$${\hat{x}}_{k|k-1}=F_k{\hat{x}}_{k-1}+B_k{u}_k$$
Observation estimation error
$${\tilde{e}}_k=z_k-{\hat{z}}_{k|k-1}$$
Observation estimation equation
$${\hat{z}}_{k|k-1}=H_k{\hat{x}}_{k|k-1}$$
Based on the above formulas, two important error covariance matrices can be defined:

State error covariance matrix
$$P_{k|k-1}=cov\{{\tilde{x}}_{k|k-1}\}$$

Observation error covariance matrix
$$S_K=cov\left\{{\tilde{e}}_k\right\}$$
The purpose of Kalman filtering is to obtain aniterative estimation expression that can be continuously corrected based on errors, and its specific form should be as follows:
$${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{\tilde{e}}_k$$
That is, the predicted value is corrected based on the error. Here $K_k$It is the Kalman gain. Therefore, we need to derive an iterative expression for the variables used and find the Kalman gain.

2. Derivation

Deriving the error covariance matrix

State error covariance matrix :

$$\begin{array}{rl}{P}_{k|k-1}& =cov\{x_k-{\hat{x}}_{k|k-1}\}\\ & =cov\{F_k{x}_{k-1}+B_k{u}_k+w_k-F_k{\hat{x}}_{k-1}-B_k{u}_k\}\\ & =cov\{F_k(x_{k-1}-{\hat{x}}_{k-1})+w_k\}\\ & =cov\left\{F_k\right(x_{k-1}-{\hat{x}}_{k-1})\}+cov\left\{w_k\right\}\\ & =F_{k}cov\{x_{k-1}-{\hat{x}}_{k-1}\}{F}_k^T+Q_k\\ & =F_k{P}_{k|k}{F}_k^T+Q_k\end{array}$$
Here we get $P_{k|k}$to $P_{k|k-1}$A recursive form of $P_{k|k}$The notation form is for the convenience of subsequent derivation, that is, to get
$$\begin{array}{r}{P}_{k-1|k-1}\to P_{k|k-1}\to P_{k|k}\end{array}$$
Similarly, the recursive form of the observation error covariance matrix can be obtained
$$\begin{array}{r}{S}_k=H_k{P}_{k|k-1}{H}_k^T+R_k\end{array}$$
It can be found that $S_k$and $S_{k-1}$There is no direct connection, which is why $S_k$Written as $S_{k|k-1}$form.

Therefore, P k ∣ k − 1 → P k ∣ k P_{k|k-1}\rightarrow P_{k|k} is still missing$P_{k|k-1}\to P_{k|k}$recursive.

Deriving the posterior covariance matrix

By definition, the error covariance $P_{k|k}$The definition of is as follows, just substitute the above definition to deduce and simplify line by line.
$$\begin{array}{rl}{P}_{k|k}& =cov\{{\tilde{x}}_{k|k}\}\\ & =cov\{x_k-{\hat{x}}_{k|k}\}\\ & =cov\{x_k-({\hat{x}}_{k|k-1}+K_k{\tilde{e}}_k)\}\\ & =cov\{x_k-({\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1}))\}\\ & =cov\{x_k-({\hat{x}}_{k|k-1}+K_k(z_k-H_k{\hat{x}}_{k|k-1}))\}\\ & =cov\{x_k-({\hat{x}}_{k|k-1}+K_k(H_k{x}_k+v_k-H_k{\hat{x}}_{k|k-1}))\}\\ & =cov\{(x_k-{\hat{x}}_{k|k-1})-K_k{H}_k(x_k-{\hat{x}}_{k|k-1})-K_k{v}_k\}\\ & =cov\{(I-K_k{H}_k)(x_k-{\hat{x}}_{k|k-1})-K_k{v}_k\}\\ & =cov\{(I-K_k{H}_k\left)\right(x_k-{\hat{x}}_{k|k-1})\}+cov\left\{K_k{v}_k\right\}\\ & =(I-K_k{H}_k)cov\{(x_k-{\hat{x}}_{k|k-1})\}(I-K_k{H}_k{)}^T+K_{k}cov\{v_k\}{K}_k^T\\ & =(I-K_k{H}_k)P_{k|k-1}(I-K_k{H}_k{)}^T+K_k{R}_k{K}_k^T\end{array}$$
Among them, the measurement error $v_k$Irrelevant to other items, so can be raised separately. This formula for any Kalman gain $K_k$Both hold, if $K_k$is the optimal Kalman gain, then it can be further simplified.

Derivation of Optimal Kalman Gain

The Kalman filter is a minimum mean square error estimator, and the posterior state error estimate is $x_k-{\hat{x}}_{k|k}$

Minimize the expected value $E[|X_k-{\hat{x}}_{k|k}{|}^2]$, which is equivalent to minimizing the posterior estimated covariance matrix$P_{k|k}$trace. Expand and offset the terms in the above equation:
$$\begin{array}{rl}{P}_{k|k}& =P_{k|k-1}-K_k{H}_k{P}_{k|k-1}-P_{k|k-1}{H}_k^T{K}_k^T+K_k{H}_k{P}_{k|k-1}{H}_k^T{K}_k^T+K_k{R}_k{K}_k^T\\ & =P_{k|k-1}-K_k{H}_k{P}_{k|k-1}-P_{k|k-1}{H}_k^T{K}_k^T+K_k(H_k{P}_{k|k-1}{H}_k^T+R_k)K_k^T\\ & =P_{k|k-1}-K_k{H}_k{P}_{k|k-1}-P_{k|k-1}{H}_k^T{K}_k^T+K_k{S}_k{K}_k^T\end{array}$$
When the matrix derivative is 0, get $P_{k|k}$The minimum value of the trace:
$$\begin{array}{r}\frac{\mathrm{d}tr(P_{k|k})}{\mathrm{d}{K}_k}=-2(H_k{P}_{k|k-1}{)}^T+2K{\_}_k{S}_k=0\end{array}$$
A commonly used formula must be used here, as follows: [Reference 1] [Reference 2] [Reference 3]
$$\begin{array}{r}\frac{\mathrm{d}tr(BAC)}{dA}=B^T{C}^T\end{array}$$
Then the Kalman gain can be solved:
$$\begin{array}{r}{K}_k=P_{k|k-1}{H}_k^T{S}_k^{-1}\end{array}$$
This gain is called the optimal Kalman gain and when used results in the minimum mean square error.

Simplification of Posterior Error Covariance Formula

Multiply S k K k T S_kK^T_k on both sides of the optimal Kalman gain formula$S_k{K}_k^T$得到
$$\begin{array}{r}{K}_k{S}_k{K}_k^T=P_{k|k-1}{H}_k^T{K}_k^T\end{array}$$
According to the posterior error covariance expansion formula, there is
$$\begin{array}{rl}{P}_{k|k}& =P_{k|k-1}-K_k{H}_k{P}_{k|k-1}-P_{k|k-1}{H}_k^T{K}_k^T+K_k{S}_k{K}_k^T\\ & =P_{k|k-1}-K_k{H}_k{P}_{k|k-1}\end{array}$$
In practice, this formula is often used due to its simple calculation, but it should be noted that this formula is only valid when the optimal Kalman gain is used. If the arithmetic precision is always so low that numerical stability is a problem, or if suboptimal Kalman gains are deliberately used, then this simplification cannot be used; the a posteriori error covariance formula derived above must be used.

references

An Introduction to the Kalman Filter

Probably the most clearly explained Kalman filter

【EKF】Kalman filter principle

Kalman Filter Wiki Encyclopedia

The most complete formula derivation of Kalman filter

The Trace of Matrix and Proof of Related Properties

Trace - Wikipedia

[Machine Learning] Summary and Detailed Explanation: The Trace of the Matrix and the Derivation of the Trace to the Matrix