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
通过采样点来拟合概率分布,直接舍弃高斯分布