Understanding and analysis of state estimation and Kalman filter

Supplementary materials ： Mean and covariance of Gaussian system （ To be added ）

Reference vision SLAM The sixth and tenth of the fourteen lectures

One . State estimation

1. Bayes' rule

A joint probability density can be decomposed into the product of a conditional probability density and a non conditional probability density ：
$p(x,y)=p(x|y)\cdot p(y)=p(y|x)\cdot p(x)$

Arrange the above formula , You can get Bayes' formula ：
$p(x|y)=\frac{p(y|x)\cdot p(x)}{p(y)}$

In Bayesian formula ,$p(x)$ Known as a priori ,$p(y|x)$ It's called likelihood , It is also a sensor model ,$p(x|y)$ It's a posterior probability . What we generally require is this a posteriori probability , It is worth noting that , In solving this a posteriori probability, we generally do not consider $p(y)$, This is because this part is related to the state to be estimated x irrelevant , So you can just ignore .

2.SLAM State estimation in

We will only discuss the simplest systems ： Linear Gaussian system , If it's not a linear Gaussian system , We can approximate nonlinear non Gaussian systems to linear Gaussian systems .
Suppose we now have an equation of motion and an equation of observation , The formula is as follows
$$\begin{gathered} x_k=f(x_{k-1},u_k)+{\omega }_k \\ {z}_k=h(x_k)+{\nu }_k \end{gathered}$$
Some points to pay attention to ：
1. The equation of motion is given by IMU Wait for the sensor to get , The observation equation is obtained by sensors such as cameras . All the state quantities are unified to $x_k$ in , Contains the current k A carrier of time pose And a large number of feature points landmark. For the equation of motion , The state quantity at the current time is generally only one of the carrier pose（ There are no characteristic points in the equation of motion , about IMU, There may be speed v And offset bias）. No matter how many state variables are included , In the end, they are unified into one $x_k$ in .
2. Suppose that the noise of the two equations is a Gaussian distribution satisfying the zero mean （ That's normal distribution ）：
${\omega }_k\sim N(0,R_k),{\nu }_k\sim N(0,Q_k)$

2.1 How to estimate the state by probability

The preceding equation can be expressed by the following conditional probability distribution , That is to say It is known that k Time motion data u And observation data z Under the condition of , Find the conditional probability distribution of the state quantity ：
$P(x_k|z_k,u_k)$
First of all, simply understand , There is no equation of motion , Only the observation equation , That is, the conditional probability becomes
$P(x_k|z_k)$
According to the Bayes formula , The formula can be decomposed
$P(x_k|z_k)=\frac{P(z_k|x_k)\cdot P(x_k)}{P(z_k)}\approx P(z_k|x_k)\cdot P(x_k)$

here transcendental $P(x_k)$ Equivalent to an initial value , For example, it is initial pose And initial landmark The location of （ Also, or VIO A priori residuals in , Here is the least square model corresponding to the prior probability ）, This priori is optional . here likelihood $P(z_k|x_k)$ Is the observation model of the camera （ Re project the model , Corresponding VIO Camera re projection residuals in , This is the least square model corresponding to the likelihood ）. here Posterior probability $P(x_k|z_k)$ It is equivalent to passing the latest k Time observation and a priori optimal current k The result of the state quantity at the moment . seek The back-end probability maximization is （MAP, maximize a posterior） Is to solve the optimal state quantity , It is said that it is difficult to solve directly , So it's converted into a solution Maximization of likelihood and a priori product . It's simpler , If there is no a priori （ In fact, it is the same to have a priori , Is to add a priori residual term to the residual equation ）. That is to ask for Maximum likelihood estimation （MLE,Maximize likelihood estimation）. The transformational relationship is ：
Find the optimal state quantity –》 Find the maximum a posteriori probability –》 Find the maximum likelihood estimation

The solution of maximum likelihood estimation is relatively simple , The sensor model , Here is the re projection residual equation of the camera .

How to calculate the maximum likelihood estimation is detailed in vision SLAM Fourteen 6.1.2.
The core idea is to do the maximum likelihood estimation -log operation , Finally found , As long as the residual and covariance of the sensor model are calculated, the state quantity can be estimated .

2.2 Consider a priori probability to estimate the state

front 2.1 The state is estimated without considering a priori , Now we need to consider , We want to use the past 1 To k To estimate the current state distribution ：
$P(x_k|x_0,u_{1:k},z_{1:k})$
Explain this probability distribution a little bit , and 2.1 Of $P(x_k|z_k,u_k)$ Different , Here I want to estimate k The state quantity of time $x_k$, Is in Known initial $x_0$,1 To k All the motion data at any time u And observation data z Under the condition of , Find the conditional probability distribution of the state quantity ,
Or according to the Bayes formula , The formula can be decomposed ：
$P(x_k|x_0,u_{1:k},z_{1:k})\approx P(z_k|x_k)\cdot P(x_k|x_0,u_{1:k},z_{1:k-1})$
The formula is based on $P(x_k|z_k)=\frac{P(z_k|x_k)\cdot P(x_k)}{P(z_k)}\approx P(z_k|x_k)\cdot P(x_k)$ Put... Directly $z_k$ and $x_k$ Swapped positions , The rest of the conditional variables remain unchanged , Simple point , You just take out the rest of the conditional variables , Discovery and $P(x_k|z_k)=\frac{P(z_k|x_k)\cdot P(x_k)}{P(z_k)}\approx P(z_k|x_k)\cdot P(x_k)$ Will be exactly the same .

Understanding of the new Bayes formula ：
here transcendental $P(x_k|x_0,u_{1:k},z_{1:k-1})$ Contains the information of all previous moments , The message is ： Initial state quantity $x_0,1 To k Time motion data u, as well as 1 To k-1 The observation equation of time z. here likelihood $P(z_k|x_k)$ and 2.1 The likelihood is the same , Are observation models of cameras . here Posterior probability $P(x_k|x_0,u_{1:k},z_{1:k})$ and 2.1 Different , It includes k Time and all previous data , And the initial state $x_0$.

For a priori consideration , Now there are two theories ：

（1） First order Markov , Think k The state of the moment is k-1 The state of the moment is related to , It has nothing to do with all the previous states , This is the filtering method for state estimation , In the filtering method , We will estimate from the state at a certain time , Deduce the state of the next moment .

（2） consider k The time state is related to the estimation of all previous states , That is, the method of nonlinear optimization .

3. Linear systems and KF

Let's put 2.2 A posteriori probability formula ：

$P(x_k|x_0,u_{1:k},z_{1:k})\approx P(z_k|x_k)\cdot P(x_k|x_0,u_{1:k},z_{1:k-1})$

To solve this probability formula , The likelihood and a priori probability need to be calculated separately , And then multiply it to get xk A posteriori probability of time .

Before that, we have the following two key points ：

（1） Let's talk about simple Linear Gaussian system . Then the equations of motion and observation can be described by the following linear equations ：

$\begin{array}{rl}{x}_k& =A_k{x}_{k-1}+u_k+w_k\\ z_k& =C_k{x}_k+v_k\end{array}$

Or suppose that the noise of the two equations is a Gaussian distribution satisfying the zero mean （ That's normal distribution ）：
${\omega }_k\sim N(0,R_k),{\nu }_k\sim N(0,Q_k)$

（2） about KF Come on , Gaussian systems only need to be considered and maintained mean value and covariance that will do , The mean value is the specific result of the state quantity updated iteratively , The covariance is the distribution of the state quantity at that time .

3.1 Let's look at the probability of likelihood

Likelihood probability is relatively simple , Since the observation equation is a linear Gaussian system , So the likelihood probability is ：

$P(z_k|x_k)=N(C_k{x}_k,Q_k)$

$C_k{x}_k$ Is the mean of the observations ,$Q_k$ Is the covariance of the observations

3.2 And then take out the prior alone - forecast

$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})\cdot P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})d{x}_{k-1}$

According to the first-order Markov property , You can rewrite the above formula ：

$\begin{array}{rl}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})\cdot P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})d{x}_{k-1}\\ & =\int P(x_k|x_{k-1},u_k)\cdot P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})d{x}_{k-1}\\ & =\int P(x_k|x_{k-1},u_k)d{x}_{k-1}\cdot P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})d{x}_{k-1}\end{array}$

among $\int P(x_k|x_{k-1},u_k)d{x}_{k-1}$ yes The probability of the equation of motion ,$P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})d{x}_{k-1}$ yes k-1 A posteriori probability of time . that k The significance of a priori probability of time is k The probability of the equation of motion at time times k-1 A posteriori probability of time .

Let me just say a word here ,k The posterior probability of time is k Probability of observation equation at time , multiply k Probability of the equation of motion at time , And ride on k-1 A posteriori probability of time . and k The posterior probability of time contains k The state and covariance of time

hypothesis k-1 A posteriori probability of time by ：

$P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})d{x}_{k-1}=N({\hat{x}}_{k-1},{\hat{P}}_{k-1})$

that k A priori probability of time
$P(x_k|x_0,u_{1:k},z_{1:k-1})=N(A_k{\hat{x}}_{k-1}+u_k,{\hat{A}}_k{P}_{k-1}{A}_k^T+R_k)$

Under the framework of Kalman filter ,k A priori probability of time Namely forecast , Its physical meaning is that the probability of the equation of motion is multiplied by k-1 A posteriori probability of time .

3.3 to update

Update is calculation k A posteriori probability of time . Its physical meaning is k The probability of the observation equation at time is multiplied by k A priori probability of time . See visual for specific calculation method derivation slam Fourteen 10.1.2 The formula 10.14-10.23, Finally, the inter model vision of Kalman filter slam Fourteen formulas 10.24-10.26.

The Kalman filtering process of a simple linear Gaussian system is divided into two steps , The first is the prediction process , The prediction process is to consider the probability of the equation of motion at the current time and the posterior probability of the previous time , So we can get the current probability of prediction , That is, the current a priori probability . The second is to update , Update is to consider the observation at the current time and the prior probability at the current time , Get the posterior probability of the current moment , The posterior probability is the probability of the current moment , It contains the mean and covariance of the state quantity at the current time .