Slam Kalman filter & nonlinear optimization

SLAM Kalman filtering && Nonlinear optimization

Gaussian distribution （ Normal distribution ） Is a common continuous probability distribution . A mathematical expectation or expectation of a normal distribution $\mu$ Equal to the position parameter , Determines the location of the distribution ; Its variance ${\sigma }^2$ The square root or standard deviation of $\sigma$ Equal to scale parameter , It determines the magnitude of the distribution .

1. Gaussian distribution

A random variable $x$ Obey Gauss distribution $N$, Then its probability density function is ：
$$p(x)=\frac{1}{\sigma \sqrt{2\pi }}exp(-\frac{(x-\mu {)}^2}{2{\sigma }^2})$$
Its high dimensional form is ：
$$P(x)=\frac{1}{\sqrt{(2\pi {)}^{N}det(\Sigma )}}exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))$$

2. Operation of Gaussian distribution

2.1 Linear operation

Let two independent Gaussian distributions ：
$$x\sim N({\mu }_x,{\Sigma }_{xx}),y\sim N({\mu }_y,{\Sigma }_{yy})$$
So their sum is still a Gaussian distribution ：
$$x+y\sim N({\mu }_x+{\mu }_y,{\Sigma }_{xx}+{\Sigma }_{yy})$$
If you take a constant $a$ multiply $x$, that $ax$ Satisfy ：
$$ax\sim N({\mu }_{a}x,a^2{\Sigma }_{xx})$$
If you take $y=Ax$, that $y$ Satisfy ：
$$y\sim N({\mu }_{A}x,A^2{\Sigma }_{xx})$$

2.2 The product of

Let the product of two Gaussian distributions satisfy $p(xy)=N(\mu ,\Sigma )$, that ：
$$\begin{gathered} {\Sigma }^{-1}={\Sigma }_{xx}^{-1}+{\Sigma }_{yy}^{-1} \\ {\Sigma }_{\mu }={\Sigma }_{xx}^{-1}{\mu }_x+{\Sigma }_{yy}^{-1}{\mu }_y \end{gathered}$$

2.2 Compound operation

At the same time, consider $x$ and $y$, When they are not independent , The composite distribution is ：
$$p(x,y)=N\left(\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right]\right)$$
among ${\Sigma }_{yx}$ and ${\Sigma }_{yx}$ Is the covariance of two variables , Satisfy ：
$$Cov(X,Y)=E[(X-E(X))(Y-E(Y))]=E[XY]-E[X]E[Y]$$
The covariance is positive , It means that these two variables are positively correlated , Zero is irrelevant , Negative is negative correlation .
By conditional distribution （ Bayes' rule ） Expansion $p(x,y)=p(x|y)p(y)$ Can be launched , Conditional probability $p(x|y)$ Satisfy ：
$$p(x|y)=N({\mu }_x+{\Sigma }_{xx}{\Sigma }_{yy}^{-1}(y-{\mu }_y),{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx})$$

---

Shuerbu

Now let's deduce how to reach this conclusion ：
Shure complement theory ： Decompose the matrix into upper triangular matrix 、 Diagonal matrix 、 The form of product of lower triangular matrix , Easy to calculate , namely ：
$$\begin{gathered} \left[\begin{array}{cc}A& B\\ C& D\end{array}\right] \\ =\left[\begin{array}{cc}I& B{D}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}\Delta D& 0\\ 0& D\end{array}\right]\left[\begin{array}{cc}I& 0\\ D^{-1}C& I\end{array}\right] \end{gathered}$$
among $\Delta D=A-B{D}^{-1}C$ It's called a matrix $D$ Of Shuerbu .
similarly ：
$$\begin{gathered} {\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]}^{-1} \\ =\left[\begin{array}{cc}I& 0\\ -D^{-1}C& I\end{array}\right]\left[\begin{array}{cc}\Delta D^{-1}& 0\\ 0& D^{-1}\end{array}\right]\left[\begin{array}{cc}I& -B{D}^{-1}\\ 0& I\end{array}\right] \end{gathered}$$

---

Specific derivation process （ understand ）

because $p(x|y)=p(x,y)/p(y)$, According to its exponential partial expansion form, we can get ：
$$\begin{gathered} {\left(\left[\begin{array}{c}x\\ y\end{array}\right]-\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right]\right)}^T{\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right]}^T\left(\left[\begin{array}{c}x\\ y\end{array}\right]-\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right]\right) \\ ={\left(\left[\begin{array}{c}x\\ y\end{array}\right]-\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right]\right)}^T\left[\begin{array}{cc}1& 0\\ -{\Sigma }_{yy}^{-1}{\Sigma }_{yx}& 1\end{array}\right]\left[\begin{array}{cc}({\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx}{)}^{-1}& 0\\ 0& {\Sigma }_{yy}^{-1}\end{array}\right]\left[\begin{array}{cc}1& -{\Sigma }_{xy}{\Sigma }_{yy}^{-1}\\ 0& 1\end{array}\right]\left(\left[\begin{array}{c}x\\ y\end{array}\right]-\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right]\right) \\ ={\left(x-{\mu }_x-{\Sigma }_{xx}{\Sigma }_{yy}^{-1}(y-{\mu }_y)\right)}^T({\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx}{)}^T{\left(x-{\mu }_x-{\Sigma }_{xx}{\Sigma }_{yy}^{-1}(y-{\mu }_y)\right)}^T+(y-{\mu }_y{)}^T{\Sigma }_{yy}^{-1}(y-{\mu }_y) \end{gathered}$$
because $p(y)$ The index part of is ：
$$(y-{\mu }_y{)}^T{\Sigma }_{yy}^{-1}(y-{\mu }_y)$$
therefore ,
$$p(x|y)=p(x,y)/p(y)=N({\mu }_x+{\Sigma }_{xx}{\Sigma }_{yy}^{-1}(y-{\mu }_y),{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx})$$

---

3. Examples of compound operations

Consider random variables $x\sim N({\mu }_x,{\Sigma }_{xx})$, Another variable $y$ Satisfy ：
$$y=Ax+b+w$$
among $A,b$ Is the coefficient matrix and offset of the linear variable ,$w$ Is the noise term , Gaussian distribution with zero mean ：$w\sim N(0,R)$. therefore ：
$$\begin{gathered} E(y)=E(Ax+b+w)=AE(x)+b+E(w)=A{\mu }_x+b \\ Cov(y)=E[(x-E[x])(x-E(x))]+Cov(w)=AE[(x-{\mu }_x)(x-{\mu }_x{)}^T]A^T+R=A{\Sigma }_{xx}{A}^T+R \end{gathered}$$
$$p(y)=N(A{\mu }_x+b,A{\Sigma }_{xx}{A}^T+R)$$

4.SLAM The mathematical representation of the problem

Usually , A robot or an unmanned vehicle will carry a sensor to measure its own motion （IMU, Wheel speed odometer, etc ）, Through integral operation, we can abstract a mathematical model about ontology perception ：
$$x_k=f(x_{k-1},u_k,w_k)$$
there $u_k$ Is the movement sensor reading （ Sometimes called input ）,$w_k$ Is noise , This equation is called Equation of motion , At present, there is no specific sports significance , Because the motion sensor can be selected , Different equations can be listed according to different motion sensors , Or a combination of them , That is, multi-sensor fusion .
alike , stay SLAM There is also an environment sensing sensor in the （ Laser radar 、 The camera 、 Infrared equipment ） The observation part of , The corresponding equation of environmental perception is The observation equation , When a robot or an unmanned car is $x_k$ A road marking is observed at the position of $y_j$, At this point, an observation data is generated z_{k,j}, Similarly, the mathematical model is abstracted as ：
$$z_{k,j}=h(y_j,x_k,v_{k,j})$$
among ,$v_{k,j}$ To observe noise , Similarly, the observation equation has different forms according to different environmental sensors .
therefore SLAM The problem can be summed up in two basic equations ：
$$\{\begin{array}{lr}{x}_k=f(x_{k-1},u_k,w_k)& \\ z_{k,j}=h(y_j,x_k,v_{k,j})& \end{array}$$
It can be seen from the above equation ：
1. In the observation equation , Only in position $x_k$ I saw it $y_j$ The observation data will be generated , Otherwise, there will be no . But in practice , A small number of road signs can usually be seen at one location , At the same time, whether it is a camera or a lidar to observe features , There are many feature points , So in the actual process The observation equation will be much larger than the number of motion equations .
2. In the equation of motion , In the actual process , If you do pure vision SLAM Or pure laser SLAM, The equation of motion is often missing ; In this case, it is usually assumed that the vehicle is moving at a constant speed or at a constant speed or at a constant acceleration .
3. Postures $x$（ location ） And road signs $y$（ Drawing ） as A random variable that obeys a certain probability distribution , Not a single number . Since both are quantities to be estimated , Change the mark , Make $x_k$ by $k$ All the unknowns of time , At the same time, it includes the vehicle posture at the current time （ It is assumed that the calibration of the sensor and the vehicle body has been completed ） And m A road mark , be ：
$$x_k\triangleq \{x_k,y_1,......,y_m\}$$
In the same way $k$ All observations at all times are recorded as $z_k$, therefore ：
$$\{\begin{array}{lr}{x}_k=f(x_{k-1},u_k)+w_k& \\ z_k=h(x_k)+v_{k,j}& \end{array}$$
The posterior probability density to be estimated is directly given as ：
$$P(x_k|x_0,u_{1:k},z_{1:k})$$
among ：
$x_0$ The initial value representing the state ;$u_{1:k}$ Express $1$ To $k$ Input of motion sensor ;$z_{1:k}$ It means $0$ To $k$ All observations at all times , The question means ：** Based on all historical data ( Input 、 observation 、 The initial state ) Get the final fusion result , That is, the filtering problem .** It is shown by a graph as ：

according to Bayes The laws of , take $z_k$ and $x_k$ Swap places ：
$$P(x_k|x_0,u_{1:k},z_{1:k})\propto P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k-1})$$
Note here ： The first item is called likelihood , The second item is called transcendental . In different ways , The prior part is actually obtained by motion prediction , be called forecast part , There are also sections based on all past states called transcendental , as follows , according to $x_k-1$ Time is the conditional probability expansion ：
$$P(x_k|x_0,u_{1:k},z_{1:k-1})=\int P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})d{x}_{k-1}$$
For this formula , This is called （A） The formula , Draw the following conclusions and assumptions ：
1. If we think about what we were like a long time ago , You can also continue to expand this formula .
2. If you only care about $k$ Time and $k-1$ moment , We can assume that First order Markov property ：$k$ The state of the moment is only related to $k-1$ Time state is irrelevant , It has nothing to do with the previous state , Get the filtering method , Such as Extended Kalman filter （EKF）： Estimate by the state at a certain time , To the next moment .
3. If you think about $k$ Time status and before All States The connection of , obtain An optimization framework based on nonlinear optimization .

5. Kalman filtering （KF）

Yes （A） The formula makes Markov assumption , The current moment is only related to the previous moment , The forecast part is ：
$$P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})=P(x_k|x_{k-1},u_k)$$
The prior part can be reduced to ：
$$P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})=P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})$$
As can be seen from the figure, the input quantity $u_k$ And $k-1$ Time independent . The two equations above ： With $k-1$ State estimation of time , Deduce to $k$ moment .
hypothesis ：
1. The state quantity here obeys the Gaussian distribution , It only needs to consider the mean and covariance of the maintenance state quantity .
2. All States and noises satisfy Gaussian distribution .
Linear Gauss system can be expressed by motion equation and observation equation ：
$$\{\begin{array}{lr}{x}_k=A_k{x}_{k-1}+u_k+w_k& \\ z_k=C_k{x}_k+v_k& \end{array}$$
The noise obeys the zero mean Gaussian distribution , be ：
$$w_k\sim N(0,\mathbf{R}),v_k\sim N(0,\mathbf{Q})$$
remember $\hat{x}$ Is posterior ,$\overline{x}$ Is a priori distribution , Suppose that $k-1$ A posteriori state estimation of time ：${\hat{x}}_{k-1}$ And the covariance at this time ${\hat{\mathbf{P}}}_{k-1}$. The purpose at this time is relatively clear ： according to $k-1$ A posteriori state estimation of time , And input and observation data determination $k$ A posteriori distribution of time .
1. Forecast part
according to Equation of motion and Gaussian distribution Determined as ：
$$P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})=N({\mathbf{A}}_k{\hat{x}}_{k-1}+u_k,{\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^T+\mathbf{R})$$
remember ：
$$\overline{x}={\mathbf{A}}_k{\hat{x}}_{k-1}+u_k,\overline{\mathbf{P}}={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^T+\mathbf{R}$$
2. The observation equation
The observation equation can be calculated in a certain state , Data should be observed , according to The observation equation and Gaussian distribution Determined as ：
$$P(z_k|x_k)=N(C_k{x}_k,Q)$$
Let the result be $x_k\sim N({\hat{x}}_k,{\hat{\mathbf{P}}}_k)$, According to the formula ：
$$P(x_k|x_0,u_{1:k},z_{1:k})\propto P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k-1})$$
You can get a formula familiar with the previous derivation ：
$$N({\hat{x}}_k,{\hat{\mathbf{P}}}_k)=N({\overline{x}}_k,{\overline{\mathbf{P}}}_k)\cdot N({\mathbf{C}}_k{x}_k,\mathbf{Q})=N\left(\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right]\right)$$
Use the conclusion directly ：
$$N({\hat{x}}_k,{\hat{\mathbf{P}}}_k)=N({\mu }_x+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y-{\mu }_y),{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx})$$
You can see... Directly here ：
$$\begin{gathered} {\overline{\mathbf{P}}}_k={\Sigma }_{xx} \\ {\Sigma }_{yy}=\Sigma (C_k{x}_k)+\Sigma (n)=E[(C_k{x}_k-u_x)(C_k{x}_k-u_x{)}^T]+\mathbf{Q}=C_k{\overline{\mathbf{P}}}_k{C}_k^T+\mathbf{Q} \end{gathered}$$
Can be derived
$$\begin{gathered} {\Sigma }_{xy}=E[(x-u_x)(z_k-u_z{)}^T] \\ =E[(x-u_x)(C_k{x}_k-C_k{u}_x+n{)}^T)] \\ =E[(x-u_x)(C_k{x}_k-C_k{u}_x{)}^T+(x-u_x)n^T] \\ ={\Sigma }_{xx}{C}_k^T \\ ={\overline{\mathbf{P}}}_k{C}_k^T \\ {\Sigma }_{yx}={\Sigma }_{xy}^T=C_k{\overline{\mathbf{P}}}_k \end{gathered}$$
Definition ：
$$\mathbf{K}={\Sigma }_{xy}{\Sigma }_{yy}^{-1}={\overline{\mathbf{P}}}_k{C}_k^T(C_k{\overline{\mathbf{P}}}_k{C}_k^T+\mathbf{Q}{)}^{-1}$$
Then there are
$$\begin{gathered} {\hat{\mathbf{P}}}_k={\overline{\mathbf{P}}}_k-\mathbf{K}{C}_k{\overline{\mathbf{P}}}_k=(\mathbf{I}-\mathbf{K}{C}_k){\overline{\mathbf{P}}}_k \\ {\hat{x}}_k={\overline{x}}_k+\mathbf{K}(z_k-C_k\overline{x}) \end{gathered}$$
Sort it out , The five equations are ：

1. forecast ：$$\overline{x}={\mathbf{A}}_k{\hat{x}}^{k-1}+u_k,\overline{{\mathbf{P}}_{\mathbf{k}}}={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^T+\mathbf{R}$$
2. to update ： To calculate $\mathbf{K}$, Also known as Kalman gain ：
$$\mathbf{K}={\overline{\mathbf{P}}}_k{C}_k^T(C_k{\overline{\mathbf{P}}}_k{C}_k^T+\mathbf{Q}{)}^{-1}$$
Then calculate the posterior probability distribution ：$$\begin{gathered} {\hat{\mathbf{P}}}_k=(\mathbf{I}-\mathbf{K}{C}_k){\overline{\mathbf{P}}}_k \\ {\hat{x}}_k={\overline{x}}_k+\mathbf{K}(z_k-C_k\overline{x}) \end{gathered}$$

thus , We deduce the whole process of the classical Kalman filter . In fact, there are several ways to deduce Kalman filter , We use the form of maximum posterior probability estimation from the perspective of probability . We see , In a linear Gaussian system , Kalman filter constitutes the maximum posterior probability estimation in the system . and , Because the Gaussian distribution is
After linear transformation, it still obeys Gaussian distribution , So we didn't make any approximations in the whole process . so to speak , Kalman filter constitutes the best unbiased estimation of linear system .

6. Extended Kalman filter （EKF）

Kalman filter is usually applied to linear systems , But in SLAM in , The equations of motion and observation are usually nonlinear functions . Regardless of vision SLAM The model is still a laser SLAM, Use lie algebra to represent the pose , It's even less likely to be a linear system . A Gaussian distribution , After nonlinear transformation , It's often no longer Gaussian , So in nonlinear systems , Approximate treatment is required , A non Gaussian distribution is approximated to a Gaussian distribution .
The results of Kalman filter are usually extended to nonlinear systems , It is called extended Kalman filter （Extended Kalman Filter,EKF）, The usual way is , Consider the first-order Taylor expansion of the equation of motion and the observation equation near a point , Only the first-order term is retained , The linear part , Then it is deduced according to the linear system . The nonlinear equations of motion and observation are ：
Equation of motion ：
$$x_k=f(x_{k-1},u_k,w_k)$$
The observation equation is ：
$$z_k=h(x_k,n_k)$$

Make $k-1$ The mean and covariance matrix at the time point is ${\hat{\mathbf{x}}}_{k-1}$,${\hat{\mathbf{P}}}_{k-1}$. stay $k$ moment , Combine the equation of motion with the equation of observation , stay ${\hat{\mathbf{x}}}_{k-1}$,${\hat{\mathbf{P}}}_{k-1}$ Linearization at （ Equivalent to first-order Taylor expansion ）：
$${x_k\approx f({\hat{x}}_{k-1},u_k,w_k)+\frac{\partial f}{\partial x_{k-1}}|}_{{\hat{x}}_{k-1}}(x_{k-1}-{\hat{x}}_{k-1})+{\frac{\partial f}{\partial w_k}|}_{{\hat{x}}_{k-1}}{w}_k$$
Note that the partial derivative here is ：
$${\mathbf{F}=\frac{\partial f}{\partial x_{k-1}}|}_{{\hat{x}}_{k-1}}$$
$${\mathbf{B}}_{k-1}={\frac{\partial f}{\partial w_k}|}_{{\hat{x}}_{k-1}}$$
For the observation equation , Similarly, there are ：
$${z_k\approx h({\overline{x}}_k)+\frac{\partial h}{\partial x_k}|}_{{\overline{x}}_k}(x_k-{\overline{x}}_k)+{\frac{\partial h}{\partial n_k}|}_{{\overline{x}}_k}{n}_k$$
remember :
$${\mathbf{H}=\frac{\partial h}{\partial x_k}|}_{{\hat{x}}_k}$$
$$C_k={\frac{\partial h}{\partial n_k}|}_{{\overline{x}}_k}$$
The expansion linearization result equation is ：
$$E[x_k]=f({\hat{x}}_{k-1},u_k,0)+{\mathbf{F}}_{k-1}(x_{k-1}-{\hat{x}}_{k-1})+E[{\mathbf{B}}_{k-1}{w}_k]$$
here $w_k$ and $n_k$ Obey the zero mean distribution ：
$$w_k\sim N(0,\mathbf{R}),n_k\sim N(0,\mathbf{Q})$$
therefore $E[{\mathbf{B}}_{k-1}{w}_k]=0$,
here $E(x_{k-1}]={\hat{x}}_{k-1}$$E({\hat{x}}_{k-1})={\hat{x}}_{k-1}$
The result is ：
$$E[x_k]=f({\hat{x}}_{k-1},u_k,0)$$
The covariance matrix is ：
$$\begin{array}{rl}Cov[x_k]& =E[(x_{k-1}-E[x_{k-1}])(x_{k-1}-E[x_{k-1}]{)}^T]+Cov[{\mathbf{B}}_{k-1}{w}_k]\\ & =\mathbf{F}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}^T+{\mathbf{B}}_{k-1}{\mathbf{R}}_k{\mathbf{B}}_{k-1}^T\end{array}$$
* The derivation here is based on the transitivity of Gaussian distribution covariance ：
$$\begin{gathered} E(y)=E(Ax+b+w)=AE(x)+b+E(w)=A{\mu }_x+b \\ Cov(y)=E[(x-E[x])(x-E(x))]+Cov(w)=AE[(x-{\mu }_x)(x-{\mu }_x{)}^T]A^T+R=A{\Sigma }_{xx}{A}^T+R \end{gathered}$$
$$Cov[{\mathbf{B}}_{k-1}{w}_k]={\mathbf{B}}_{k-1}\mathbf{C}ov[w_k]{\mathbf{B}}_{k-1}^T$$
Synthesis can lead to
$$p(x_k|x_0,u_{1:k},z_{0:k-1})=N(f({\hat{x}}_{k-1},u_k,0),\mathbf{F}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}^T+{\mathbf{B}}_{k-1}{\mathbf{R}}_k{\mathbf{B}}_{k-1}^T)$$
Similar to Kalman filter , Here you are ：
$${\overline{x}}_k=f({\hat{x}}_{k-1},u_k,0),{\overline{\mathbf{P}}}_k=\mathbf{F}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}^T+{\mathbf{B}}_{k-1}{\mathbf{R}}_k{\mathbf{B}}_{k-1}^T$$
similarly , In the observation equation ：
$$\begin{gathered} E[z_k]=E[h({\overline{x}}_k)+\mathbf{H}(x_k-{\overline{x}}_k)+{\mathbf{C}}_k{n}_k]=h({\overline{x}}_k)+\mathbf{H}(x_k-{\overline{x}}_k) \\ Cov[z_k]=E[({\overline{x}}_k-E[{\overline{x}}_k])({\overline{x}}_k-E[{\overline{x}}_k]{)}^T]+Cov[{\mathbf{C}}_k{n}_k] \\ ={\mathbf{C}}_{k}E[n_k]{\mathbf{C}}_k^T={\mathbf{C}}_k\mathbf{Q}{\mathbf{C}}_k^T \end{gathered}$$
Observed as
$$p(z_k|x_k)=N(h({\overline{x}}_k)+\mathbf{H}(x_k-{\overline{x}}_k),{\mathbf{C}}_k\mathbf{Q}{\mathbf{C}}_k^T)$$
deduction EKF For the same ：
$$N({\hat{x}}_k,{\hat{\mathbf{P}}}_k)=N({\overline{x}}_k,{\overline{\mathbf{P}}}_k)\cdot N(h({\overline{x}}_k)+\mathbf{H}(x_k-x_{k-1}),{\mathbf{C}}_k\mathbf{Q}{\mathbf{C}}_k^T)=N\left(\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right]\right)$$
Also use the conclusion directly ：
Use the conclusion directly ：
$$N({\hat{x}}_k,{\hat{\mathbf{P}}}_k)=N({\mu }_x+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y-{\mu }_y),{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx})$$
You can see... Directly here ：
$$\begin{gathered} {\overline{\mathbf{P}}}_k={\Sigma }_{xx} \\ {\Sigma }_{yy}=\Sigma (h({\overline{x}}_k))+\Sigma (H_k(x_k-{\overline{x}}_k))+\Sigma ({\mathbf{C}}_k{n}_k)=0+\mathbf{H}E[(x_{k-1}-E[x_{k-1}])(x_{k-1}-E[x_{k-1}]{)}^T]{\mathbf{H}}^T+{\mathbf{C}}_k\mathbf{Q}{\mathbf{C}}_k^T=\mathbf{H}{\overline{\mathbf{P}}}_k{\mathbf{H}}^T+{\mathbf{C}}_k\mathbf{Q}{\mathbf{C}}_k^T \end{gathered}$$
Can be derived
$$\begin{gathered} {\Sigma }_{xy}=E[(x-u_x)(z_k-u_z{)}^T] \\ =E[(x-u_x)(h({\overline{x}}_k)+\mathbf{H}(x_k-{\overline{x}}_k)+\mathbf{C}{n}_k-h({\overline{x}}_k)] \\ =E[(x-u_x)({\mathbf{H}}_k{x}_k-{\mathbf{H}}_k{u}_x{)}^T+(x-u_x)n^T] \\ ={\Sigma }_{xx}{\mathbf{H}}_k^T \\ ={\overline{\mathbf{P}}}_k{\mathbf{H}}_k^T \\ {\Sigma }_{yx}={\Sigma }_{xy}^T={\mathbf{H}}_k{\overline{\mathbf{P}}}_k \end{gathered}$$
Define the Kalman gain ：
$${\mathbf{K}}_k={\Sigma }_{xy}{\Sigma }_{yy}^{-1}={\overline{\mathbf{P}}}_k{H}_k^T(H_k{\overline{\mathbf{P}}}_k{H}_k^T+{\mathbf{C}}_k\mathbf{Q}{\mathbf{C}}_k^T{)}^{-1}$$
Based on Kalman gain ：
$${\hat{x}}_k={\overline{x}}_k+{\mathbf{K}}_k(z_k-h({\overline{x}}_k))$$
$${\hat{\mathbf{P}}}_k={\overline{\mathbf{P}}}_k-{\mathbf{K}}_k{\mathbf{H}}_k{\overline{\mathbf{P}}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\overline{\mathbf{P}}}_k$$
Sum up , obtain EKF The five classical formulas of are ：

1. forecast ：
$${\overline{x}}_k=f({\hat{x}}_{k-1},u_k,0),{\overline{\mathbf{P}}}_k=\mathbf{F}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}^T+{\mathbf{B}}_{k-1}{\mathbf{R}}_k{\mathbf{B}}_{k-1}^T$$
2. to update ： To calculate $\mathbf{K}$, Also known as Kalman gain ：
$${\mathbf{K}}_k={\Sigma }_{xy}{\Sigma }_{yy}^{-1}={\overline{\mathbf{P}}}_k{H}_k^T(H_k{\overline{\mathbf{P}}}_k{H}_k^T+{\mathbf{C}}_k\mathbf{Q}{\mathbf{C}}_k^T{)}^{-1}$$
Then calculate the posterior probability distribution ：
$$\begin{gathered} {\hat{\mathbf{P}}}_k={\overline{\mathbf{P}}}_k-{\mathbf{K}}_k{\mathbf{H}}_k{\overline{\mathbf{P}}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\overline{\mathbf{P}}}_k \\ {\hat{x}}_k={\overline{x}}_k+{\mathbf{K}}_k(z_k-h({\overline{x}}_k)) \end{gathered}$$

thus , Extended Kalman filter （EKF） The derivation has been completed ,EKF Be concise in form 、 Widely used , In the early SLAM Has occupied a dominant position for a long time , however EKF There are some limitations ：

---

1. The filter assumes Markov property to some extent ：$k$ The state of the moment is only related to $k-1$ Time related , And with the $k-1$ The previous state has nothing to do with the observation （ Or related to the first few finite time states ). This assumption leads to laser / The visual odometer only considers the relationship between two adjacent frames , In case of loopback , Difficult to deal with , This requires Nonlinear optimization All historical data are put together for optimization , But the disadvantage is that it increases the amount of calculation , Consume more computing resources .
2.EKF The filter is only used in ${\hat{x}}_{k-1}$ A linearization is done at , And then directly based on the linearization results , Calculate a posterior probability . This method considers the linearized approximation at this point , The posterior probability is still valid . But in fact , When we are far away from the work site , The first-order Taylor expansion does not necessarily approximate the entire function , This depends on the nonlinearity of the motion model and the observation model . If they have strong nonlinearity , Then the linear approximation only holds in a very small range , It can't be considered that it can still be approximated by linearity in far places . This is it. EKF Nonlinear error of , Is its main problem .
3. In terms of program implementation ,EKF It is necessary to store the mean and variance of the state quantity , And maintain and update them . If you put the road signs into the State , Because of vision SLAM There are a large number of middle road signs , This amount of storage is considerable , And increases squarely with the state quantity （ Because you want to store the covariance matrix ）. therefore ,EKF SLAM It is generally considered unsuitable for large-scale scenarios .

---

7. Iterative extended Kalman filter （IEKF）

Because of the linearization approximation in the nonlinear model , When the degree of nonlinearity is stronger , The error will be large . however , Since the linearized operating point is closer to the true value , The smaller the linearization error , So one way to solve this problem is , Find the exact linearization point gradually through iteration , So as to improve the accuracy .
stay EKF In the derivation of , Only change the linearized observation point of the observation ：
$$z_k\approx h(x_{op,k},0)+{\mathbf{H}}_k(x_k-x_{op,k})+{\mathbf{C}}_k{n}_k$$
Define the Kalman gain ：
$${\mathbf{K}}_k={\Sigma }_{xy}{\Sigma }_{yy}^{-1}={\overline{\mathbf{P}}}_k{H}_k^T(H_k{\overline{\mathbf{P}}}_k{H}_k^T+{\mathbf{C}}_k\mathbf{Q}{\mathbf{C}}_k^T{)}^{-1}$$
Based on Kalman gain ：
$${\hat{x}}_k={\overline{x}}_k+{\mathbf{K}}_k((z_k-H_k{\overline{x}}_k)-(z_{op,k}-H({\overline{x}}_k))$$
During filtering , Repeat this 2 A formula , Take the posterior mean of the last time as the linear working point of this time , The purpose of reducing nonlinear error can be achieved . It should be noted that , In this filtering mode , A posteriori variance should be carried out in the last step .

8. Advantages and disadvantages of other filters

8.1 MSCKF

MSCKF The goal is solve EKF Dimension explosion problem of , That is to say EKF The third point of the discussion . Tradition EKF-SLAM Add feature points to the state vector and IMU State is estimated together , When the environment is very big , There will be many characteristic points , The dimension of the state vector becomes very large .SCKF Instead of adding feature points to the state vector , Instead, the positions and poses at different times are added to the state vector , Feature points will be seen by multiple sensors , Thus in multiple sensor states （Multi-State） Form geometric constraints between （Constraint）, Then the observation model is constructed by using geometric constraints EKF Conduct update. Because the number of sensor pose will be much smaller than the number of feature points ,MSCKF The dimensions of the state vector are compared to EKF-SLAM Greatly reduced , The historical camera state will be continuously removed , Maintain only a fixed number of camera poses （Sliding Window）, Thus to MSCKF The calculation amount of the back end is limited .
Study reference links and papers ：
1.A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation
2.Improving the Accuracy of EKF-Based Visual-Inertial Odometry
3. MSCKF Those things （ One ）MSCKF Introduction to the algorithm
4. MSCKF Those things （ Two ）S-MSCKF Trial and source code analysis

9. Kalman filtering - Discretization of system equations

Discrete derivation process ：
Given continuous time linear stochastic systems （ Linear stochastic differential equation ）：
$$\dot{\mathbf{X}}(t)=\mathbf{F}(t)\mathbf{X}(t)+\mathbf{G}(t)w(t)$$
According to linear system theory , The equivalent discretization form of the above formula is ：
$${\mathbf{X}}_k={\Phi }_{k/k-1}{\mathbf{X}}_{k-1}+{\eta }_{k-1}$$
In the formula above ,
$$\begin{gathered} {\mathbf{X}}_k=\mathbf{X}(t_k) \\ {\Phi }_{k/k-1}=\Phi (t_k,t_{k-1})\approx e^{{\int }_{t_{k-1}}^{t_k}\mathbf{F}(\tau )d\tau } \\ {\eta }_{k-1}={\int }_{t_{k-1}}^{t_k}\Phi (t_k,\tau )\mathbf{G}(\tau )w(\tau )d\tau \end{gathered}$$
Note the discrete time interval $T_s=t_k-t_{k-1}$, When $\mathbf{F}(t)$ In a short integral interval $[t_{k-1},t_k]$ When the internal change is not too drastic , And set $\mathbf{F}(t_{k-1})T_s\ll \mathbf{I}$, Then the further transfer matrix can be approximated as follows ：
$${\Phi }_{k/k-1}=\Phi (t_k,t_{k-1})\approx e^{\mathbf{F}(t_{k-1})T_s}=\mathbf{I}+\mathbf{F}(t_{k-1})T_s+{\mathbf{F}}^2(t_{k-1})\frac{T_s^2}{2!}+{\mathbf{F}}^3(t_{k-1})\frac{T_s^3}{3!}.......\approx \mathbf{I}+\mathbf{F}(t_{k-1})T_s$$
${\eta }_{k-1}$ Is a linear transformation of Gaussian white noise , The result is still a random vector with normal distribution , So we can use the mean value , variance , That is to say , Second order statistical characteristics to describe and equivalent ${\eta }_{k-1}$.
Finally, the discretized formula is as follows ：
$${\mathbf{X}}_k={\Phi }_{k/k-1}{\mathbf{X}}_{k-1}+{\Gamma }_{k-1}{w}_{k-1}$$
among ：
$${\Gamma }_{k-1}\approx [\mathbf{I}+\mathbf{F}(t_{k-1})T_s]\mathbf{G}(k-1)\approx \mathbf{G}(k-1)$$

Reference resources ：
Vision SLAM Fourteen speak , a.