SLAM--状态估计

一、 贝叶斯公式

若存在 X , Y \bm X,\bm Y X,Y随机变量,则 p ( x , y ) p(x,y) p(x,y)被称为 ( X , Y ) (\bm X,\bm Y) (X,Y)的联合概率密度函数;
边缘概率密度的性质:

性质1

p ( x , y ) = p ( x ∣ y ) ⋅ p ( y ) = p ( y ∣ x ) ⋅ p ( x ) \small p(x,y)=p(x|y)\cdot p(y)=p(y|x)\cdot p(x) p(x,y)=p(xy)p(y)=p(yx)p(x)

性质2

∫ p ( x ∣ y ) d x = 1 , ∫ p ( y ∣ x ) d y = 1 , \small \int p(x|y)\mathrm{d}x=1,\quad \int p(y|x)\mathrm{d}y=1, p(xy)dx=1,p(yx)dy=1,

性质3

p ( x ) = ∫ p ( x , y ) d y ,      p ( y ) = ∫ p ( x , y ) d x , \small p(x)=\int p(x,y)\mathrm{d}y, \quad\;\; p(y)=\int p(x,y)\mathrm{d}x, p(x)=p(x,y)dy,p(y)=p(x,y)dx,

贝叶斯公式:
p ( x ∣ y ) = p ( y ∣ x ) p ( x ) p ( y ) = p ( y ∣ x ) p ( x ) ∫ p ( y ∣ x ) p ( x ) d x p(x|y)=\frac{p(y|x)p(x)}{p(y)}=\frac{p(y|x)p(x)}{\int p(y|x)p(x)\mathrm{d}x} p(xy)=p(y)p(yx)p(x)=p(yx)p(x)dxp(yx)p(x)
对于分母 p ( y ) p(y) p(y),我们可以利用边缘密度:
p ( y ) = ∫ p ( x , y ) d y = ∫ p ( y ∣ x ) p ( x ) d x \small p(y)=\int p(x,y)\mathrm{d}y=\int p(y|x)p(x)\mathrm{d}x p(y)=p(x,y)dy=p(yx)p(x)dx
在贝叶斯推断中,我们把p(x)称为先验,p(x|y)称为后验,p(y|x)称为似然概率
 
 

二、高斯密度函数

X = [ X 1 X 2 . . . X n ] T \bm X=\begin{bmatrix} \bm X_1 &\bm X_2 &... &\bm X_n\end{bmatrix}^T X=[X1X2...Xn]T是n维随机向量, x = ( x 1 , x 2 , . . . , x n ) T \bm x = \begin{pmatrix} x_1,x_2,...,x_n\end{pmatrix}^T x=(x1,x2,...,xn)T,则 X \bm X X的联合密度函数为:
    p ( x ) = 1 ( 2 π ) n 2 ( det ⁡ B ) 1 2 exp ⁡ ( − 1 2 ( x − μ ) B − 1 ( x − μ ) ) (1) ~\\ ~\\ p(x)=\frac {1} {(2\pi)^{\frac n 2}\bm ({\det} \bm B)^{\frac 1 2}}\exp(-\frac 1 2(\bm x-\bm \mu){\bm B^{-1}(\bm x-\bm \mu) }) \tag{1}   p(x)=(2π)2n(detB)211exp(21(xμ)B1(xμ))(1)
 
记为 X ∽ N ( μ , B ) \bm X\backsim \bm N(\bm \mu,\bm B) XN(μ,B).

其中, μ = [ E X 1 , E X 2 , . . . , E X n ] T \bm \mu=\begin{bmatrix} \bm EX_1 ,\bm EX_2 ,... ,\bm EX_n\end{bmatrix}^T μ=[EX1,EX2,...,EXn]T,为各变量的数学期望或均值, B = [ b i j ] n × n \bm B=[b_{ij}]_{n\times n} B=[bij]n×n矩阵为协方差矩阵:
E X k = μ k , b k l = c o v ( X k , X l ) = E [ ( X k − E X k ) ( X l − E X l ) T ] (2) \bm E \bm X_k=\bm \mu_k, \bm b_{kl}=\bm {cov}(\bm X_k,\bm X_l)=\bm E[(\bm X_k-\bm E \bm X_k)(\bm X_l-\bm E\bm X_l)^T]\tag{2} EXk=μk,bkl=cov(Xk,Xl)=E[(XkEXk)(XlEXl)T](2)
高斯分布有一个重要的性质:

X ∽ N ( μ , B ) \bm X\backsim \bm N(\bm \mu,\bm B) XN(μ,B) C \bm C C m × n \bm {m \times n} m×n的矩阵,则m维向量 Y = C X \bm{ Y = CX} Y=CX服从m维的正态分布 N ( C μ , C B C T ) \bm {N(C\mu,CBC^T)} N(Cμ,CBCT)

 
 

三、联合高斯概率密度函数分解推导

存在多元正态分布 ( x , y ) (x,y) (x,y),则联合高斯密度函数为:
p ( x , y ) = N ( [ μ x μ y ] , [ ∑ x x ∑ x y ∑ y x ∑ y y ] ) (3) p(x,y)=\bm N(\begin{bmatrix} \bm \mu _x\\ \\\bm \mu _y \end{bmatrix},\begin{bmatrix} \bm {\sum} _{xx}& \bm {\sum} _{xy}\\ \\\bm {\sum} _{yx} & \bm {\sum} _{yy} \end{bmatrix})\tag{3} p(x,y)=N(μxμy,xxyxxyyy)(3)
其中 ∑ y x = ∑ x y T \bm {\sum} _{yx} =\bm {\sum} _{xy}^T yx=xyT

由贝叶斯法则:
p ( x , y ) = p ( x ∣ y ) ⋅ p ( y ) (4) p(x,y)=p(x|y)\cdot p(y)\tag{4} p(x,y)=p(xy)p(y)(4)
分解指数p(x,y)的部分:
( [ x y ] − [ μ x μ y ] ) T [ ∑ x x ∑ x y ∑ y x ∑ y y ] − 1 ( [ x y ] − [ μ x μ y ] )   = ( x − μ x − ∑ x y ∑ y y − 1 ( y − μ y ) ) T ⋅ ( ∑ x x − ∑ x y ∑ y y − 1 ∑ y x ) − 1 ⋅ ( x − μ x − ∑ x y ∑ y y − 1 ( y − μ y ) ) + ( y − μ y ) T ∑ y y − 1 ( y − μ y ) (5) \small \begin{aligned} &(\begin{bmatrix} \bm x\\ \\\bm y \end{bmatrix}-\begin{bmatrix} \bm \mu _x\\ \\\bm \mu _y \end{bmatrix})^T\begin{bmatrix} \bm {\sum} _{xx}& \bm {\sum} _{xy}\\ \\\bm {\sum} _{yx} & \bm {\sum} _{yy} \end{bmatrix}^{-1}(\begin{bmatrix} \bm x\\ \\\bm y \end{bmatrix}-\begin{bmatrix} \bm \mu _x\\ \\\bm \mu _y \end{bmatrix})\\ ~\\&=(\bm x-\bm \mu _x-\bm {\sum} _{xy}\bm {\sum} _{yy}^{-1}(\bm y-\bm \mu_y))^T \cdot ( \bm {\sum} _{xx}- \bm {\sum} _{xy} \bm {\sum} _{yy}^{-1} \bm {\sum} _{yx})^{-1}\cdot (\bm x-\bm \mu _x-\bm {\sum} _{xy}\bm {\sum} _{yy}^{-1}(\bm y-\bm \mu_y)) \\ &+(\bm y -\bm \mu _y)^T \bm {\sum} _{yy}^{-1} (\bm y -\bm \mu _y)\tag{5} \end{aligned}  ([xy][μxμy])Txxyxxyyy1([xy][μxμy])=(xμxxyyy1(yμy))T(xxxyyy1yx)1(xμxxyyy1(yμy))+(yμy)Tyy1(yμy)(5)
根据指数分解的结果以及 p ( x , y ) = p ( x ∣ y ) ⋅ p ( y ) p(x,y)=p(x|y)\cdot p(y) p(x,y)=p(xy)p(y)可以得到:
p ( x ∣ y ) = N ( μ x + ∑ x y ∑ y y − 1 ( y − μ y ) , ∑ x x − ∑ x y ∑ y y − 1 ∑ y x ) p ( y ) = N ( μ y , ∑ y y ) (6) \small \begin{aligned} p(x|y)&=\bm N(\bm \mu _x+ \bm {\tiny \sum} _{xy}\bm {\tiny \sum} _{yy}^{-1}(\bm y-\bm \mu_y), \bm {\tiny \sum} _{xx}- \bm {\tiny \sum} _{xy} \bm {\tiny \sum} _{yy}^{-1} \bm {\tiny \sum} _{yx})\\ \\ p(y)&=\bm N(\bm \mu_y,\bm {\tiny \sum} _{yy}) \end{aligned}\tag{6} p(xy)p(y)=N(μx+xyyy1(yμy),xxxyyy1yx)=N(μy,yy)(6)
上面的式子是非常重要的,尤其是在推导卡尔曼滤波过程中。

 
 

四、线性高斯系统状态估计

4.1 离散状态的批量估计

 
设状态方程 和 观测方程分别为:

状态方程 x k = A k x k − 1 + u k + ω k , ω k ∽ N ( 0 , Q k ) \bm x_k=\bm A_k\bm x_{k-1}+\bm u_k+\bm \omega_k,\bm \omega_k\backsim \bm N(0,\bm Q_k) xk=Akxk1+uk+ωk,ωkN(0,Qk)

观测方程 y k = C k x k + n k , n k ∽ N ( 0 , R k ) \bm y_k=\bm C_k\bm x_k+\bm n_k,\bm n_k\backsim \bm N(0,\bm R_k) yk=Ckxk+nk,nkN(0,Rk)

其中 ω k \bm \omega_k ωk n k \bm n_k nk为噪声, u k \bm u_k uk为输入, y k \bm y_k yk为观测。
 

我们利用 最大后验估计MAP 来进行推导,利用贝叶斯法则,所以MAP的优化目标为:
arg ⁡ max ⁡ x p ( x ∣ y , u ) = arg ⁡ max ⁡ x p ( y ∣ x , u ) ⋅ p ( x ∣ u ) p ( y ∣ u ) = arg ⁡ max ⁡ x p ( y ∣ x , u ) ⋅ p ( x ∣ u ) (7) \mathop {\arg\max }\limits_x p(x|y,u) =\mathop {\arg\max }\limits_x \frac {p(y|x,u) \cdot p(x|u)}{p(y|u)}=\mathop {\arg\max }\limits_x p(y|x,u)\cdot p(x|u)\tag{7} xargmaxp(xy,u)=xargmaxp(yu)p(yx,u)p(xu)=xargmaxp(yx,u)p(xu)(7)

1.首先针对其中p(y|x,u):
ln ⁡ p ( y ∣ x , u ) = − 1 2 ( y k − C k x k ) T R k − 1 ( y k − C k x k ) + m , m 和 x 不 相 关 \ln p(y|x,u)=-\frac 1 2 (y_k-C_kx_k)^TR_k^{-1}(y_k-C_kx_k)+m,m和x不相关 \\ lnp(yx,u)=21(ykCkxk)TRk1(ykCkxk)+mmx
最后优化的目标函数为
⟹ J y ( x ) = − 1 2 ( y k − C k x k ) T R k − 1 ( y k − C k x k ) (8) \Longrightarrow J_y(x)= -\frac 1 2 (y_k-C_kx_k)^TR_k^{-1}(y_k-C_kx_k)\tag{8} Jy(x)=21(ykCkxk)TRk1(ykCkxk)(8)

2.其次再针对p(x|u):
ln ⁡ p ( x ∣ u ) = − 1 2 ( x k − A k x k − 1 − u k ) T Q k − 1 ( x k − A k x k − 1 − u k ) + n , n 和 x 不 相 关 \ln p(x|u)=-\frac 1 2 (x_k-A_kx_{k-1}-u_k)^TQ_k^{-1}(x_k-A_kx_{k-1}-u_k)+n,n和x不相关 \\ lnp(xu)=21(xkAkxk1uk)TQk1(xkAkxk1uk)+nnx
最后优化的目标函数为
⟹ J u ( x ) = − 1 2 ( x k − A k x k − 1 − u k ) T Q k − 1 ( x k − A k x k − 1 − u k ) (9) \Longrightarrow J_u(x)= -\frac 1 2 (x_k-A_kx_{k-1}-u_k)^TQ_k^{-1}(x_k-A_kx_{k-1}-u_k)\tag{9} Ju(x)=21(xkAkxk1uk)TQk1(xkAkxk1uk)(9)

所以目标函数为:
J ( x ) = J u ( x ) + J y ( x )   = − 1 2 ( y k − C k x k ) T R k − 1 ( y k − C k x k ) + − 1 2 ( x k − A k x k − 1 − u k ) T Q k − 1 ( x k − A k x k − 1 − u k ) (10) J(x)=J_u(x)+J_y(x)\\~\\=-\frac 1 2 (y_k-C_kx_k)^TR_k^{-1}(y_k-C_kx_k)+-\frac 1 2 (x_k-A_kx_{k-1}-u_k)^TQ_k^{-1}(x_k-A_kx_{k-1}-u_k)\tag{10} J(x)=Ju(x)+Jy(x) =21(ykCkxk)TRk1(ykCkxk)+21(xkAkxk1uk)TQk1(xkAkxk1uk)(10)

我们把 x ^ \hat{x} x^作为后验估计, x ˇ \check{x} xˇ作为先验估计

即: x ^ = arg ⁡ max ⁡ x J ( x ) \hat{x}=\mathop {\arg\max }\limits_x J(x) x^=xargmaxJ(x)

我们令 z = [ x ˇ 0 , u 1 , . . . , u k ∣ y 0 , . . . , y k ] T \bm z=[\bm {\check{x}_0,u_1,...,u_k|y_0,...,y_k}]^T z=[xˇ0,u1,...,uky0,...,yk]T x = [ x 0 , . . . , x k ] T \bm {x=[x_0,...,x_k]^T} x=[x0,...,xk]T,并且定义:
H = [ 1 − A 0 1 − A 1 1 . . . − A k − 1 1 C 0 C 1 . . . C k − 1 C k ] \small \bm H=\begin{bmatrix} 1 \\ -A_0&1\\ &-A_1&1\\ &&...\\ &&& -A_{_{k-1} }&1\\ C_0\\&C_1\\&&...\\&&&C_{k-1}\\ &&&&C_k\end{bmatrix} H=1A0C01A1C11......Ak1Ck11Ck
W = [ Q R ] \small \bm W=\begin{bmatrix} \bm Q\\&\bm R \end{bmatrix} W=[QR]
这时我们的目标函数可以写成:
J ( x ) = 1 2 ( z − H x ) T W − 1 ( z − H x ) (11) \small \bm J(x)=\frac 1 2 (\bm z-\bm Hx)^T\bm W^{-1}(\bm z-\bm Hx)\tag{11} J(x)=21(zHx)TW1(zHx)(11)
由于W矩阵是对称矩阵,对目标函数求导可得:
∂ J ( x ) ∂ x = ( H x ^ − z ) T W − 1 H (12) \small \frac {\partial J(x)}{\partial x}=(\bm H\hat{x}-\bm z)^T\bm W^{-1}\bm H\tag{12} xJ(x)=(Hx^z)TW1H(12)
令导数等于0:
( H x ^ − z ) T W − 1 H = 0   ⟹ H T W − 1 ( H x ^ − z ) = 0 \small (\bm H\hat{x}-\bm z)^T\bm W^{-1}\bm H=0\\ ~\\ \Longrightarrow \bm H^T\bm W^{-1}(\bm H\hat{x}-\bm z)=0 (Hx^z)TW1H=0 HTW1(Hx^z)=0
所以:
( H T W − 1 H ) x ^ = H T W − 1 ⋅ z (13) \small (\bm H^T \bm W^{-1}\bm H)\hat{x}=\bm H^T\bm W^{-1}\cdot \bm z\tag{13} (HTW1H)x^=HTW1z(13)

4.2 最大后验估计的协方差

根据贝叶斯法则,可以表示为:
p ( x ∣ z ) = β exp ⁡ ( − 1 2 ( H x − z ) T W − 1 ( H x − z ) ) , β 为 归 一 化 因 子 (14) \small p(x|z)=\beta\exp(-\frac 1 2 (\bm Hx-\bm z)^T\bm W^{-1}(\bm Hx-\bm z)),\beta 为归一化因子\tag{14} p(xz)=βexp(21(Hxz)TW1(Hxz)),β(14)
联合式13可得:
p ( x ∣ x ^ ) = κ exp ⁡ ( − 1 2 ( x − x ^ ) T ( H T W − 1 H ) ( x − x ^ ) ) (15) p(x|\hat{x})=\kappa\exp(-\frac 1 2 (x-\hat{x})^T(\bm H^T \bm W^{-1}\bm H)(x-\hat{x}))\tag{15} p(xx^)=κexp(21(xx^)T(HTW1H)(xx^))(15)
x ∽ N ( x ^ , P ^ ) x\backsim \bm N(\hat{x},\hat{P}) xN(x^,P^)可以推断:

其中: ( H T W − 1 H ) − 1 = P ^ \small(\bm H^T \bm W^{-1}\bm H)^{-1}=\hat{P} (HTW1H)1=P^,即协方差;

4.3 利用稀疏性求解

至此,就可以求出x的最大后验估计,但我们不会直接求 ( H T W − 1 H ) − 1 \small(\bm H^T \bm W^{-1}\bm H)^{-1} (HTW1H)1,一般的方法是采用Cholesky分解或者利用矩阵的稀疏性来解。下面介绍Cholesky分解法:

注意到:
H T W − 1 H = [ ∗ ∗ ∗ ∗ ∗ ∗ ∗ ∗ . . . . . . ∗ ∗ ∗ ∗ ∗ ] \small \bm H^T \bm W^{-1}\bm H=\begin{bmatrix}*&*&\\ *&*&*\\&*&*&*\\ &&&.&.&.\\ &&&&.&.&.\\ &&&&&*&*&*\\ &&&&&&*&* \end{bmatrix} HTW1H=......
根据Cholesky分解法将其分解为:
H T W − 1 H = L L T (16) \small \bm H^T \bm W^{-1}\bm H=\bm L \bm L^T\tag{16} HTW1H=LLT(16)
其中L为下三角矩阵:

L = [ ∗ ∗ ∗ ∗ ∗ . . . . . . ∗ ∗ ∗ ∗ ] \small \bm L =\begin{bmatrix} *&\\ *&*\\&*&*&\\ &&.&.&.\\ &&&.&.&.\\ &&&&&*&*&\\ &&&&&&*&* \end{bmatrix} L=......
这样我们就可以先求解:
L d = H T W − 1 ⋅ z (17) \small \bm Ld=\bm H^T\bm W^{-1}\cdot \bm z\tag{17} Ld=HTW1z(17)
之后求解:
L T x ^ = d \small \bm L^T\hat{x}=d LTx^=d
由于矩阵的特殊性,我们可以直接将上一步计算的结果直接代入到下一步直接求解,这里不再展开,具体参考:《机器人学中的状态估计》----p48-52

参考博文:

视觉SLAM中的数学——解方程AX=b与矩阵分解:奇异值分解(SVD分解) 特征值分解 QR分解 三角分解 LLT分解
 
 

五、离散卡尔曼滤波

5.1 线性高斯系统

存在状态方程和观测方程:
状态方程 x k = A k x k − 1 + u k + ω k , ω k ∽ N ( 0 , Q k ) \bm x_k=\bm A_k\bm x_{k-1}+\bm u_k+\bm \omega_k,\bm \omega_k\backsim \bm N(0,\bm Q_k) xk=Akxk1+uk+ωk,ωkN(0,Qk)

观测方程 y k = C k x k + n k , n k ∽ N ( 0 , R k ) \bm y_k=\bm C_k\bm x_k+\bm n_k,\bm n_k\backsim \bm N(0,\bm R_k) yk=Ckxk+nk,nkN(0,Rk)
 
 

5.1.1 利用联合高斯概率密度推导

由贝叶斯公式:
p ( x k , y k ∣ u k ) = p ( x k ∣ y k , u k ) ⋅ p ( y k ∣ u k ) (18) p(x_k,y_k|u_k)=p(x_k|y_k,u_k)\cdot p(y_k|u_k)\tag{18} p(xk,ykuk)=p(xkyk,uk)p(ykuk)(18)
由本文的第三节可以知道,当我们求出联合分布概率密度时,就能求出边缘概率密度;所以我们对 p ( x k , y k ∣ u k ) p(x_k,y_k|u_k) p(xk,ykuk)进行求解:
p ( x k , y k ∣ u k ) = N ( [ μ x μ y ] , [ ∑ x x ∑ x y ∑ y x ∑ y y ] ) = N ( [ x k ˇ C k x k ˇ ] , [ ∑ x x ∑ x y ∑ y x ∑ y y ] ) p(x_k,y_k|u_k)=\bm N(\begin{bmatrix} \bm \mu _x\\ \\\bm \mu _y \end{bmatrix},\begin{bmatrix} \bm {\sum} _{xx}& \bm {\sum} _{xy}\\ \\\bm {\sum} _{yx} & \bm {\sum} _{yy} \end{bmatrix})=\bm N(\begin{bmatrix} \bm {\check{x_k}} \\ \\ \bm C_k\bm {\check{x_k}} \end{bmatrix},\begin{bmatrix} \bm {\sum} _{xx}& \bm {\sum} _{xy}\\ \\\bm {\sum} _{yx} & \bm {\sum} _{yy} \end{bmatrix}) p(xk,ykuk)=N(μxμy,xxyxxyyy)=N(xkˇCkxkˇ,xxyxxyyy)
其中
∑ x y = E [ ( x k − E x k ) ( y k − E y k ) T ] = E [ ( x k − x ˇ k ) ( C k x k − C k x ˇ k ) T ]   ⟹ ∑ x y = ∑ x x ⋅ C k T = P ˇ k ⋅ C k T (19) \small \bm {\sum} _{xy}=\bm E[(x_k-\bm Ex_k)(y_k-\bm Ey_k)^T]=E[(x_k-\bm {\check{x}_k})(\bm C_k\bm x_k-\bm C_k\bm {\check{x}_k})^T]\\ ~\\ \Longrightarrow \bm {\sum} _{xy}= \bm {\sum} _{xx}\cdot \bm C_k^T=\bm {\check{P}_k}\cdot \bm C_k^T \tag{19} xy=E[(xkExk)(ykEyk)T]=E[(xkxˇk)(CkxkCkxˇk)T] xy=xxCkT=PˇkCkT(19)
由于对称矩阵,所以 ∑ y x = ∑ x y T = C k ⋅ P ˇ k \bm {\sum} _{yx}= \bm {\sum} _{xy}^T= \bm C_k\cdot \bm {\check{P}_k} yx=xyT=CkPˇk

所以:
p ( x k , y k ∣ u k ) = N ( [ x k ˇ C k x k ˇ ] , [ P ˇ k P ˇ k ⋅ C k T C k ⋅ P ˇ k C k P ˇ k C k T + R k ] ) (20) p(x_k,y_k|u_k)=\bm N(\begin{bmatrix} \bm {\check{x_k}} \\ \\ \bm C_k\bm {\check{x_k}} \end{bmatrix},\begin{bmatrix} \bm {\check{P}_k}& \bm {\check{P}_k}\cdot \bm C_k^T\\ \\\bm C_k\cdot \bm {\check{P}_k} & \bm C_k\bm{\check{P}_k}\bm C_k^T+\bm R_k \end{bmatrix})\tag{20} p(xk,ykuk)=N(xkˇCkxkˇ,PˇkCkPˇkPˇkCkTCkPˇkCkT+Rk)(20)

所以很快求出:
p ( x k ∣ y k , u k ) = N ( μ x + ∑ x y ∑ y y − 1 ( y − μ y ) , ∑ x x − ∑ x y ∑ y y − 1 ∑ y x )   ⟹ = N ( x ˇ k + P ˇ k ⋅ C k T ( C k P ˇ k C k T + R k ) − 1 ( y k − C k x ˇ k ) , ( 1 − P ˇ k ⋅ C k T ( C k P ˇ k C k T + R k ) − 1 C k ) P ˇ k ) (21) \small p(x_k|y_k,u_k)=\bm N(\bm \mu _x+ \bm {\tiny \sum} _{xy}\bm {\tiny \sum} _{yy}^{-1}(\bm y-\bm \mu_y), \bm {\tiny \sum} _{xx}- \bm {\tiny \sum} _{xy} \bm {\tiny \sum} _{yy}^{-1} \bm {\tiny \sum} _{yx})\\ ~\\ \Longrightarrow = \bm N(\check{x}_k+\bm {\check{P}_k}\cdot \bm C_k^T(\bm C_k\bm{\check{P}_k}\bm C_k^T+\bm R_k)^{-1}(y_k-\bm C_k\check{x}_k),(1-\bm {\check{P}_k}\cdot \bm C_k^T(C_k\bm{\check{P}_k}\bm C_k^T+\bm R_k)^{-1}\bm C_k)\bm {\check{P}_k})\tag{21} p(xkyk,uk)=N(μx+xyyy1(yμy),xxxyyy1yx) =N(xˇk+PˇkCkT(CkPˇkCkT+Rk)1(ykCkxˇk),(1PˇkCkT(CkPˇkCkT+Rk)1Ck)Pˇk)(21)
 
------------更新------------
为了简化,我们令:
K k = P ˇ k ⋅ C k T ( C k P ˇ k C k T + R k ) − 1   P ^ k = ( 1 − K k C k ) P k ˇ   x ^ k = x ˇ k + K k ( y k − C k x ˇ k ) (22) K_k=\bm {\check{P}_k}\cdot \bm C_k^T(\bm C_k\bm{\check{P}_k}\bm C_k^T+\bm R_k)^{-1}\\ ~\\ \bm {\hat{P}_k}=(1-K_k\bm C_k)\bm {\check{P_k}}\\ ~\\ \bm {\hat{x}_k}= \bm {\check{x}_k}+K_k(\bm y_k-\bm C_k\bm {\check{x}_k})\tag{22} Kk=PˇkCkT(CkPˇkCkT+Rk)1 P^k=(1KkCk)Pkˇ x^k=xˇk+Kk(ykCkxˇk)(22)
K k K_k Kk我们也称为卡尔曼增益

------------预测------------
其中:
{ x ˇ k = A k x ^ k − 1 + u k   P k ˇ = A k P ^ k − 1 A k T + Q k (23) \left \{ \begin{aligned} &\bm {\check{x}_k=A_k \hat{x}_{k-1}+u_k}\\ ~\\ &\bm {\check{P_k}}=\bm{A_k\hat{P}_{k-1}A_k^T+Q_k}\\ \end{aligned} \right.\tag{23}  xˇk=Akx^k1+ukPkˇ=AkP^k1AkT+Qk(23)
上面就是卡尔曼滤波的更新过程
 
 

5.1.2 最大后验估计推导

由贝叶斯法则得:
arg ⁡ max ⁡ x k   p ( x k ∣ y k , u k ) = arg ⁡ max ⁡ x k p ( y k ∣ x k , u k ) ⋅ p ( x k ∣ u k ) p ( y k ∣ u k )   = arg ⁡ max ⁡ x k p ( y k ∣ x k , u k ) ⋅ p ( x k ∣ u k ) (24) \small\begin{aligned} \mathop {\arg\max }\limits_{x_k} \ p(x_k|y_k,u_k)&=\mathop {\arg\max }\limits_{x_k}\frac{p(y_k|x_k,u_k)\cdot p(x_k|u_k)}{p(y_k|u_k)}\\ ~\\ &=\mathop {\arg\max }\limits_{x_k} p(y_k|x_k,u_k)\cdot p(x_k|u_k)\tag{24} \end{aligned} xkargmax p(xkyk,uk) =xkargmaxp(ykuk)p(ykxk,uk)p(xkuk)=xkargmaxp(ykxk,uk)p(xkuk)(24)
p ( y k ∣ u k ) p(y_k|u_k) p(ykuk)与要估计的 x k x_k xk无关,所以可以直接忽略。其中: p ( x k ∣ y k , u k ) p(x_k|y_k,u_k) p(xkyk,uk)后验概率 p ( y k ∣ x k , u k ) p(y_k|x_k,u_k) p(ykxk,uk)似然 p ( x k ∣ u k ) p(x_k|u_k) p(xkuk)先验概率

但直接求后验是非常困难的,所以我们需要来求后者的概率分布:

1.对于先验分布
p ( x k ∣ u k ) = N ( x ˇ k , P ˇ k )   其 中 : x ˇ k = A k x ^ k − 1 + u k , P ˇ k = A k P ^ k − 1 A k T + Q k (25) \small p(x_k|u_k)=\bm N(\bm {\check{x}_k},\bm {\check{P}_k})\\ ~\\ 其中:\qquad \bm {\check{x}_k=A_k\hat{x}_{k-1}+u_k},\quad \bm{\check{P}_k=A_k\hat{P}_{k-1}A_k^T+Q_k}\tag{25} p(xkuk)=N(xˇk,Pˇk) :xˇk=Akx^k1+uk,Pˇk=AkP^k1AkT+Qk(25)
2.似然部分
p ( y k ∣ x k , u k ) = N ( C k x k , R k ) (26) p(y_k|x_k,u_k)=\bm N(\bm C_k \bm x_k,\bm R_k)\tag{26} p(ykxk,uk)=N(Ckxk,Rk)(26)

3.后验估计部分
设有:
p ( x k ∣ y k , u k ) = N ( x ^ k , P ^ k )   ∴ N ( x ^ k , P ^ k ) = μ N ( C k x k , R k ) ⋅ N ( x ˇ k , P ˇ k ) (27) p(x_k|y_k,u_k)=\bm N(\bm {\hat{x}_k},\bm {\hat{P}_k})\\ ~\\ \therefore \bm N(\bm {\hat{x}_k},\bm {\hat{P}_k})=\mu \bm N(\bm C_k \bm x_k,\bm R_k)\cdot \bm N(\bm {\check{x}_k},\bm {\check{P}_k}) \tag{27} p(xkyk,uk)=N(x^k,P^k) N(x^k,P^k)=μN(Ckxk,Rk)N(xˇk,Pˇk)(27)
对于高斯分布来说,我们一般不会在意前面的因子部分,更注重的是高斯密度函数指数展开部分,因为高斯分布的指数部分包含了所有均值协方差信息。
 
( x k − x ^ k ) T P ^ k − 1 ( x k − x ^ k ) = ( y k − C k x k ) R k − 1 ( y k − C k x k ) + ( x k − x ˇ k ) T P ˇ k − 1 ( x k − x ˇ k ) (28) \small \bm{(x_k-\hat{x}_k)^T\hat{P}_k^{-1}(x_k-\hat{x}_k)=(y_k-C_kx_k)R_k^{-1}(y_k-C_kx_k)+(x_k-\check{x}_k)^T\check{P}_k^{-1}(x_k-\check{x}_k)} \tag{28} (xkx^k)TP^k1(xkx^k)=(ykCkxk)Rk1(ykCkxk)+(xkxˇk)TPˇk1(xkxˇk)(28)
我们考虑 x k \bm x_k xk二次型的系数:
P ^ k − 1 = C k T R k − 1 C k + P ˇ k − 1 (29) \bm{\hat{P}_k^{-1}=C_k^{T}R_k^{-1}C_k+\check{P}_k^{-1}} \tag{29} P^k1=CkTRk1Ck+Pˇk1(29)
我们定义一个中间量: K k = P ^ k C k T R k − 1 K_k=\bm {\hat{P}_k}\bm C_k^{T}\bm R_k^{-1} Kk=P^kCkTRk1
∴ P ^ k = ( 1 − K k C k ) P k ˇ (30) \therefore\quad \bm {\hat{P}_k}=(1-K_k\bm C_k)\bm {\check{P_k}} \tag{30} P^k=(1KkCk)Pkˇ(30)
再分析一次系数:
− 2 x ^ k T P ^ k − 1 x k = − 2 y k T R k − 1 C k x k − 2 x ˇ k T P ˇ k − 1 x k (31) -2\bm {\hat{x}_k^T\hat{P}_k^{-1}x_k}=-2 \bm {y_k^TR_k^{-1}C_kx_k}-2\bm {\check{x}_k^T\check{P}_k^{-1}x_k} \tag{31} 2x^kTP^k1xk=2ykTRk1Ckxk2xˇkTPˇk1xk(31)
根据系数并转置,最后表示 x k ^ \bm {\hat{x_k}} xk^得:
x ˇ k + K k ( y k − C k x ˇ k ) (32) \bm {\check{x}_k}+K_k(\bm y_k-\bm C_k\bm {\check{x}_k}) \tag{32} xˇk+Kk(ykCkxˇk)(32)
 
 

5.1.3 卡尔曼滤波迭代算法

根据推导,我们的卡尔曼滤波迭代过程为:
------------第一步:预测------------
{ x ˇ k = A k x ^ k − 1 + u k   P k ˇ = A k P ^ k − 1 A k T + Q k (33) \small \left \{ \begin{aligned} &\bm {\check{x}_k=A_k \hat{x}_{k-1}+u_k}\\ ~\\ &\bm {\check{P_k}}=\bm{A_k\hat{P}_{k-1}A_k^T+Q_k}\\ \end{aligned} \right.\tag{33}  xˇk=Akx^k1+ukPkˇ=AkP^k1AkT+Qk(33)
------------第二步:更新------------
{ K k = P ˇ k ⋅ C k T ( C k P ˇ k C k T + R k ) − 1   P ^ k = ( 1 − K k C k ) P k ˇ   x ^ k = x ˇ k + K k ( y k − C k x ˇ k ) (34) \small \left \{ \begin{aligned} K_k&=\bm {\check{P}_k}\cdot \bm C_k^T(\bm C_k\bm{\check{P}_k}\bm C_k^T+\bm R_k)^{-1}\\ ~\\ \bm {\hat{P}_k}&=(1-K_k\bm C_k)\bm {\check{P_k}}\\ ~\\ \bm {\hat{x}_k}&= \bm {\check{x}_k}+K_k(\bm y_k-\bm C_k\bm {\check{x}_k}) \end{aligned} \right.\tag{34} Kk P^k x^k=PˇkCkT(CkPˇkCkT+Rk)1=(1KkCk)Pkˇ=xˇk+Kk(ykCkxˇk)(34)
卡尔曼滤波是最大后验概率估计,主要根据是高斯分布的线性变换依然是高斯分布,另外高斯密度函数也可以进行相应的线性变换。
 
 

5.2 非线性高斯系统

存在非线性的状态方程和观测方程:

状态方程 x k = f ( x k − 1 , u k ) + ω k , ω k ∽ N ( 0 , Q k ) \bm x_k=f(\bm x_{k-1},\bm u_k)+\bm \omega_k,\bm \omega_k\backsim \bm N(0,\bm Q_k) xk=f(xk1,uk)+ωk,ωkN(0,Qk)

观测方程 y k = h ( x k ) + n k , n k ∽ N ( 0 , R k ) \bm y_k=h(\bm x_k)+\bm n_k,\bm n_k\backsim \bm N(0,\bm R_k) yk=h(xk)+nk,nkN(0,Rk)

由泰勒展开式:
在这里插入图片描述

线性化
f ( x k − 1 , u k ) ≈ f ( x ^ k − 1 , u k ) + ∂ f ( x ^ k − 1 , u k ) ∂ x k − 1 ⋅ ( x k − 1 − x ^ k − 1 ) = x ˇ k + ∂ f ∂ x k − 1 ∣ x ^ k − 1 , u k ⋅ ( x k − 1 − x ^ k − 1 ) f(x_{k-1 },u_k)\approx f(\hat{x}_{k-1},u_k)+\frac {\partial f(\bm{\hat{x}_{k-1},u_k}) } {\partial \bm x_{k-1}}\cdot(x_{k-1}-\hat{x}_{k-1})=\check{x}_k+\frac {\partial f } {\partial \bm x_{k-1}}\Bigg|_{\bm{\hat{x}_{k-1},u_k}}\cdot(x_{k-1}-\hat{x}_{k-1}) f(xk1,uk)f(x^k1,uk)+xk1f(x^k1,uk)(xk1x^k1)=xˇk+xk1fx^k1,uk(xk1x^k1)
h ( x k ) ≈ y ˇ k + ∂ h ∂ x k ∣ x ˇ k ( x k − x ˇ k ) h(x_k)\approx\check{y}_k+\frac {\partial h } {\partial \bm x_k} \Bigg|_{\bm{\check{x}_{k}}}(x_{k}-\check{x}_{k}) h(xk)yˇk+xkhxˇk(xkxˇk)

我们不再详细推导过程,我们令:
F = ∂ f ∂ x k − 1 ∣ x ^ k − 1 , u k   H = ∂ h ∂ x k ∣ x ˇ k \bm F=\frac {\partial f } {\partial \bm x_{k-1}}\Bigg|_{\bm{\hat{x}_{k-1},u_k}} \\ ~\\\bm H=\frac {\partial h } {\partial \bm x_k} \Bigg|_{\bm{\check{x}_{k}}} F=xk1fx^k1,uk H=xkhxˇk

------------第一步:预测------------
{ x ˇ k = f ( x ^ k − 1 , u k )   P k ˇ = F P ^ k − 1 F T + Q k (35) \small \left \{ \begin{aligned} &\bm {\check{x}_k= f(\bm {\hat{x}_{k-1}},\bm u_k) }\\ ~\\ &\bm {\check{P_k}}=\bm{F\hat{P}_{k-1}F^T+Q_k}\\ \end{aligned} \right.\tag{35}  xˇk=f(x^k1,uk)Pkˇ=FP^k1FT+Qk(35)
------------第二步:更新------------
{ K k = P ˇ k ⋅ H T ( H P ˇ k H T + R k ) − 1   P ^ k = ( 1 − K k H ) P k ˇ   x ^ k = x ˇ k + K k ( y k − h ( x ˇ k ) ) (36) \small \left \{ \begin{aligned} K_k&=\bm {\check{P}_k}\cdot \bm H^T(\bm H\bm{\check{P}_k}\bm H^T+\bm R_k)^{-1}\\ ~\\ \bm {\hat{P}_k}&=(1-K_k\bm H)\bm {\check{P_k}}\\ ~\\ \bm {\hat{x}_k}&= \bm {\check{x}_k}+K_k(\bm y_k-\bm h(\bm {\check{x}_k})) \end{aligned} \right.\tag{36} Kk P^k x^k=PˇkHT(HPˇkHT+Rk)1=(1KkH)Pkˇ=xˇk+Kk(ykh(xˇk))(36)
至此,卡尔曼滤波的算法就推导完成。

  • 0
    点赞
  • 21
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值