重新整理KF相关公式，Byes滤波器、KF、EKF、IEKF、UKF、PF
Filter
Bayes filter：
使用概率分布推导，适用于离散系统
P ( x ∣ z 1 , … , z n ) = P ( z n ∣ x , z 1 , … , z n − 1 ) P ( x ∣ z 1 , … , z n − 1 ) P ( z n ∣ z 1 , … , z n − 1 ) P\left(x | z_{1}, \ldots, z_{n}\right)=\frac{P\left(z_{n} | x, z_{1}, \ldots, z_{n-1}\right) P\left(x | z 1, \ldots, z_{n-1}\right)}{P\left(z_{n} | z_{1}, \ldots, z_{n-1}\right)} P(x∣z1,…,zn)=P(zn∣z1,…,zn−1)P(zn∣x,z1,…,zn−1)P(x∣z1,…,zn−1)
x已知时，z只与x有关
P ( x ∣ z 1 , … , z n ) = P ( z n ∣ x , z 1 , … , z n − 1 ) P ( x ∣ z 1 , … , z n − 1 ) P ( z n ∣ z 1 , … , z n − 1 ) = P ( z n ∣ x ) P ( x ∣ z 1 , … , z n − 1 ) P ( z n ∣ z 1 , … , z n − 1 ) \begin{aligned} P\left(x | z_{1}, \ldots, z_{n}\right) &=\frac{P\left(z_{n} | x, z_{1}, \ldots, z_{n-1}\right) P\left(x | z 1, \ldots, z_{n-1}\right)}{P\left(z_{n} | z_{1}, \ldots, z_{n-1}\right)} \\ &=\frac{P\left(z_{n} | x\right) P\left(x | z 1, \ldots, z_{n-1}\right)}{P\left(z_{n} | z_{1}, \ldots, z_{n-1}\right)} \end{aligned} P(x∣z1,…,zn)=P(zn∣z1,…,zn−1)P(zn∣x,z1,…,zn−1)P(x∣z1,…,zn−1)=P(zn∣z1,…,zn−1)P(zn∣x)P(x∣z1,…,zn−1)
P ( x ∣ z 1 , … , z n ) = P ( z n ∣ x ) P ( x ∣ z 1 , … , z n − 1 ) P ( z n ∣ z 1 , … , z n − 1 ) = η n P ( z n ∣ x ) P ( x ∣ z 1 , … , z n − 1 ) \begin{aligned} P\left(x | z_{1}, \ldots, z_{n}\right) &=\frac{P\left(z_{n} | x\right) P\left(x | z_{1}, \ldots, z_{n-1}\right)}{P\left(z_{n} | z_{1}, \ldots, z_{n-1}\right)} \\ &=\eta_{n} P\left(z_{n} | x\right) P\left(x | z_{1}, \ldots, z_{n-1}\right) \end{aligned} P(x∣z1,…,zn)=P(zn∣z1,…,zn−1)P(zn∣x)P(x∣z1,…,zn−1)=ηnP(zn∣x)P(x∣z1,…,zn−1)
对于单纯的贝叶斯，对于离散系统
P ( x ∣ z 1 , … , z n ) = P ( z n ∣ x ) P ( x ∣ z 1 , … , z n − 1 ) P ( z n ∣ z 1 , … , z n − 1 ) = η n P ( z n ∣ x ) P ( x ∣ z 1 , … , z n − 1 ) = η n P ( z n ∣ x ) η n − 1 P ( z n − 1 ∣ x ) P ( x ∣ z 1 , … , z n − 2 ) = η 1 ⋯ η n ∏ i = 1 … n P ( z i ∣ x ) P ( x ) \begin{aligned} P\left(x | z_{1}, \ldots, z_{n}\right) &=\frac{P\left(z_{n} | x\right) P\left(x | z_{1}, \ldots, z_{n-1}\right)}{P\left(z_{n} | z_{1}, \ldots, z_{n-1}\right)} \\ &=\eta_{n} P\left(z_{n} | x\right) P\left(x | z_{1}, \ldots, z_{n-1}\right) \\ &=\eta_{n} P\left(z_{n} | x\right) \eta_{n-1} P\left(z_{n-1} | x\right) P\left(x | z_{1}, \ldots, z_{n-2}\right) \\ &=\eta_{1} \cdots \eta_{n} \prod_{i=1 \ldots n} P\left(z_{i} | x\right) P(x) \end{aligned} P(x∣z1,…,zn)=P(zn∣z1,…,zn−1)P(zn∣x)P(x∣z1,…,zn−1)=ηnP(zn∣x)P(x∣z1,…,zn−1)=ηnP(zn∣x)ηn−1P(zn−1∣x)P(x∣z1,…,zn−2)=η1⋯ηni=1…n∏P(zi∣x)P(x)
对于其与KF的连接：
P ( x ∣ z 1 , … , z n ) = P ( z n ∣ x ) P ( x ∣ z 1 , … , z n − 1 ) P ( z n ∣ z 1 , … , z n − 1 ) = η n P ( z n ∣ x ) P ( x ∣ z 1 , … , z n − 1 ) \begin{aligned} P\left(x | z_{1}, \ldots, z_{n}\right) &=\frac{P\left(z_{n} | x\right) P\left(x | z_{1}, \ldots, z_{n-1}\right)}{P\left(z_{n} | z_{1}, \ldots, z_{n-1}\right)} \\ &=\eta_{n} P\left(z_{n} | x\right) P\left(x | z_{1}, \ldots, z_{n-1}\right) \end{aligned} P(x∣z1,…,zn)=P(zn∣z1,…,zn−1)P(zn∣x)P(x∣z1,…,zn−1)=ηnP(zn∣x)P(x∣z1,…,zn−1)
P ( x n ∣ z 1 , … , z n − 1 ) = ∫ P ( x n ∣ x n − 1 ) P ( x n − 1 ∣ z 1 , … , z n − 1 ) d x n − 1 \begin{aligned} P\left(x_{n} | z_{1}, \ldots, z_{n-1}\right) &=\int P\left(x_{n} | x_{n-1}\right) P\left(x_{n-1} | z_{1}, \ldots, z_{n-1}\right)dx_{n-1} \end{aligned} P(xn∣z1,…,zn−1)=∫P(xn∣xn−1)P(xn−1∣z1,…,zn−1)dxn−1
KF
适用于线性高斯系统，预测和更新均为高斯分布
系统：
x k = A x k − 1 + B u k + w k y k = C x k + v k \begin{aligned} &x_{k}=A x_{k-1}+B u_{k}+w_{k}\\ &y_{k}=C x_{k}+v_{k} \end{aligned} xk=Axk−1+Buk+wkyk=Cxk+vk
Prediction
x ^ k − = A x ^ k − 1 + B u k P k − = A P k − 1 A T + Q \begin{aligned} &\hat{x}_{k}^{-}=\mathbf{A} \hat{x}_{k-1}+\mathbf{B} u_{k}\\ &P_{k}^{-}=\mathbf{A} P_{k-1} \mathbf{A}^{T}+Q \end{aligned} x^k−=Ax^k−1+BukPk−=APk−1AT+Q
update
K k = P k − C T C P k − C T + R x ^ k = x ^ k − + K k ( y k − C x ^ k − ) P k = ( I − K k C ) P k − \begin{array}{l} {K_{k}=\frac{P_{k}^{-} \mathbf{C}^{\mathrm{T}}}{\mathbf{C} P_{k}^{-} \mathbf{C}^{\mathrm{T}}+R}} \\ {\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(y_{k}-\mathbf{C} \hat{x}_{k}^{-}\right)} \\ {P_{k}=\left(I-K_{k} \mathbf{C}\right) P_{k}^{-}} \end{array} Kk=CPk−CT+RPk−CTx^k=x^k−+Kk(yk−Cx^k−)Pk=(I−KkC)Pk−
EKF
针对非线性系统，使用雅可比矩阵代替系数矩阵计算概率分布
system model
x k = f ( x k − 1 , u k ) + w k y k = h ( x k ) + v k \begin{aligned} &x_{k}=f\left(x_{k-1}, u_{k}\right)+w_{k}\\ &y_{k}=h\left(x_{k}\right)+v_{k} \end{aligned} xk=f(xk−1,uk)+wkyk=h(xk)+vk
prediction
x ^ k − = f ( x ^ k − 1 , u k ) P k − = F P k − 1 F T + Q F = ∂ f ∂ x ∣ x ^ k − 1 , u k \begin{aligned} \hat{x}_{k}^{-} &=f\left(\hat{x}_{k-1}, u_{k}\right) \\ P_{k}^{-} &=\mathbf{F} P_{k-1} \mathbf{F}^{T}+Q \\ \mathbf{F} &=\left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{k-1}, u_{k}} \end{aligned} x^k−Pk−F=f(x^k−1,uk)=FPk−1FT+Q=∂x∂f∣∣∣∣x^k−1,uk
update
S = H P k − H T + R K k = P k − H T S − 1 x ^ k = x ^ k − + K k ( y k − h ( x ^ k − ) ) P k = ( I − K k H ) P k − H = ∂ h ∂ x k − \begin{array}{l} S = \mathbf{H} P_{k}^{-} \mathbf{H}^{\mathrm{T}}+R \\ {K_{k}={P_{k}^{-} H^{T}}{S^{-1}}} \\ {\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(y_{k}-h\left(\hat{x}_{k}^{-}\right)\right)} \\ {P_{k}=\left(I-K_{k} \mathbf{H}\right) P_{k}^{-}} \\ {\mathbf{H}=\frac{\partial h}{\partial x_{k}^{-}}} \end{array} S=HPk−HT+RKk=Pk−HTS−1x^k=x^k−+Kk(yk−h(x^k−))Pk=(I−KkH)Pk−H=∂xk−∂h
- 高斯分布经过非线性化变换之后不是高斯分布
- 线性化误差
- 因为线性化导致的误差在均值的计算中进行传播，进而累积传播到协方差的计算中
UKF
直接使用sigmod points的变换对象创建拟合高斯分布对象，描述更新观测后的分布
IEKF
对update过程以似然函数创建目标函数进行优化
PF
通过采样点来拟合概率分布，直接舍弃高斯分布