模型描述
离散时间状态估计问题
运动方程:
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
观测方程:
z
k
=
h
(
x
k
)
+
v
k
\begin{split} 运动方程:\pmb x_k &=\pmb f(\pmb x_{k-1},\pmb u_k)+\pmb w_k \\ 观测方程:\pmb z_k &=\pmb h(\pmb x_k)+\pmb v_k \\ \end{split}
运动方程:xk观测方程:zk=f(xk−1,uk)+wk=h(xk)+vk
变量含义如下:
- 系统状态: x k ∈ R n \pmb x_k \in \mathbb{R}^n xk∈Rn
- 初始状态: x 0 ∈ R n ∼ N ( x ˇ 0 , P ˇ 0 ) \pmb x_0 \in \mathbb{R}^n \sim \mathcal N(\check{\pmb x}_0, \check{\pmb P}_0) x0∈Rn∼N(xˇ0,Pˇ0)
- 输入: u k ∈ R p \pmb u_k \in \mathbb{R}^p uk∈Rp
- 过程噪声: w k ∈ R n ∼ N ( 0 , R k ) \pmb w_k \in \mathbb{R}^n \sim \mathcal N(\pmb 0,\pmb R_k) wk∈Rn∼N(0,Rk)
- 测量 : z k ∈ R m \pmb z_k \in \mathbb{R}^m zk∈Rm
- 测量噪声: v k ∈ R m ∼ N ( 0 , Q k ) \pmb v_k \in \mathbb{R}^m \sim \mathcal N(\pmb 0,\pmb Q_k) vk∈Rm∼N(0,Qk)
已知数据:
- 先验初始状态及其协方差矩阵 x ˇ 0 , P ˇ 0 \check{\pmb x}_0, \check{\pmb P}_0 xˇ0,Pˇ0
- 输入量 u 1 : k \pmb u_{1:k} u1:k
- 观测量 z 0 : k \pmb z_{0:k} z0:k
- 系统的运动模型 f ( ) \pmb f() f()和观测模型 h ( ) \pmb h() h()
待估计数据:
- 系统真实状态的后验估计值 x ^ k \hat{\pmb x}_k x^k
最大后验估计(Maximum A Posteriori,MAP)
定义宏观变量:
- x = x 0 : K = ( x 0 , ⋯ x K ) \pmb x = \pmb x_{0:K} = (\pmb x_0, \cdots \pmb x_K) x=x0:K=(x0,⋯xK)
- u = ( x ˇ 0 , u 1 : K ) \pmb u = (\check{\pmb x}_0, \pmb u_{1:K}) u=(xˇ0,u1:K)
- z = z 0 : K = ( z 0 , ⋯ z K ) \pmb z = \pmb z_{0:K} = (\pmb z_0, \cdots \pmb z_K) z=z0:K=(z0,⋯zK)
利用贝叶斯公式,得到MAP估计为:
x
^
=
arg
max
x
p
(
x
∣
u
,
z
)
=
arg
max
x
p
(
z
∣
x
,
u
)
p
(
x
∣
u
)
p
(
z
∣
u
)
=
arg
max
x
p
(
z
∣
x
)
p
(
x
∣
u
)
\begin{split} \hat{\pmb x} &= \underset{\pmb x}{\arg \max} \, p(\pmb x | \pmb u, \pmb z) \\ &= \underset{\pmb x}{\arg\max} \, \frac{p(\pmb z| \pmb x,\pmb u) p(\pmb x | \pmb u)}{p(\pmb z | \pmb u)} \\ &= \underset{\pmb x}{\arg \max} \, p(\pmb z | \pmb x) p(\pmb x | \pmb u) \\ \end{split}
x^=xargmaxp(x∣u,z)=xargmaxp(z∣u)p(z∣x,u)p(x∣u)=xargmaxp(z∣x)p(x∣u)
其中:
p
(
z
∣
x
)
=
∏
k
=
0
K
p
(
z
k
∣
x
k
)
p
(
x
∣
u
)
=
p
(
x
0
∣
x
ˇ
0
)
∏
k
=
1
K
p
(
x
k
∣
x
k
−
1
,
u
k
)
\begin{split} p(\pmb z | \pmb x) &= \prod_{k=0}^K p(\pmb z_k | \pmb x_k) \\ p(\pmb x| \pmb u) &= p(\pmb x_0 | \check{\pmb x}_0) \prod_{k=1}^K p(\pmb x_k | \pmb x_{k-1}, \pmb u_k) \end{split}
p(z∣x)p(x∣u)=k=0∏Kp(zk∣xk)=p(x0∣xˇ0)k=1∏Kp(xk∣xk−1,uk)
所以:
x
^
=
arg
max
x
p
(
x
0
∣
x
ˇ
0
)
(
∏
k
=
1
K
p
(
x
k
∣
x
k
−
1
,
u
k
)
)
(
∏
k
=
0
K
p
(
z
k
∣
x
k
)
)
=
arg
max
x
(
ln
p
(
x
0
∣
x
ˇ
0
)
+
∑
k
=
1
K
ln
(
p
(
x
k
∣
x
k
−
1
,
u
k
)
)
+
∑
k
=
0
K
(
p
(
z
k
∣
x
k
)
)
)
\begin{split} \hat{\pmb x} &= \underset{\pmb x}{\arg \max} \, p(\pmb x_0 | \check{\pmb x}_0) \left(\prod_{k=1}^K p(\pmb x_k | \pmb x_{k-1}, \pmb u_k) \right) \left(\prod_{k=0}^K p(\pmb z_k | \pmb x_k) \right) \\ &= \underset{\pmb x}{\arg \max} \left( \ln p(\pmb x_0 | \check{\pmb x}_0) + \sum_{k=1}^K \ln(p(\pmb x_k | \pmb x_{k-1}, \pmb u_k)) + \sum_{k=0}^K (p(\pmb z_k | \pmb x_k)) \right) \end{split}
x^=xargmaxp(x0∣xˇ0)(k=1∏Kp(xk∣xk−1,uk))(k=0∏Kp(zk∣xk))=xargmax(lnp(x0∣xˇ0)+k=1∑Kln(p(xk∣xk−1,uk))+k=0∑K(p(zk∣xk)))
贝叶斯滤波
马尔可夫性:当一个随机过程在给定现在状态及所有过去状态情况下,其未来状态的条件概率分布仅依赖于当前状态;换句话说,在给定现在状态时,未来状态与过去(该过程的历史路径)是条件独立的,那么此随机过程称为马尔可夫过程。
根据马尔可夫性,当已知
x
k
−
1
\pmb x_{k-1}
xk−1时,可根据运动方程推出
x
k
\pmb x_k
xk
p
(
x
k
∣
x
ˇ
0
,
u
1
:
k
,
z
0
:
k
)
⏟
后验
=
η
p
(
z
k
∣
x
k
)
⏟
似然
p
(
x
k
∣
x
ˇ
0
,
u
1
:
k
,
z
0
:
k
−
1
)
⏟
先验
=
η
p
(
z
k
∣
x
k
)
∫
p
(
x
k
,
x
k
−
1
∣
x
ˇ
0
,
u
1
:
k
,
z
0
:
k
−
1
)
d
x
k
−
1
=
η
p
(
z
k
∣
x
k
)
∫
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
0
:
k
−
1
)
d
x
k
−
1
=
η
p
(
z
k
∣
x
k
)
⏟
h
(
⋅
)
进行更新
∫
p
(
x
k
∣
x
k
−
1
,
u
k
)
⏟
f
(
⋅
)
进行预测
p
(
x
k
−
1
∣
x
ˇ
0
,
u
1
:
k
−
1
,
z
0
:
k
−
1
)
⏟
先验
d
x
k
−
1
\begin{split} \underbrace{p(\pmb x_k | \check{\pmb x}_0, \pmb u_{1:k}, \pmb z_{0:k}) }_{\text 后验} &= \eta \, \underbrace{ p(\pmb z_k | \pmb x_k)}_{\text 似然} \, \underbrace{ p(\pmb x_k | \check{\pmb x}_0, \pmb u_{1:k}, \pmb z_{0:k-1}) }_{\text 先验} \\ &= \eta \, p(\pmb z_k | \pmb x_k) \, \int p(\pmb x_k,\pmb x_{k-1} | \check{\pmb x}_0, \pmb u_{1:k}, \pmb z_{0:k-1})d \pmb x_{k-1} \\ &= \eta \, p(\pmb z_k | \pmb x_k) \, \int p(\pmb x_k | \pmb x_{k-1},\check{\pmb x}_0, \pmb u_{1:k}, \pmb z_{1:k-1}) \, p(\pmb x_{k-1} | \check{\pmb x}_0, \pmb u_{1:k}, \pmb z_{0:k-1})d \pmb x_{k-1} \\ &= \eta \,\underbrace{ p(\pmb z_k | \pmb x_k) }_{\text h({\cdot})进行更新} \, \int \underbrace{ p(\pmb x_k | \pmb x_{k-1},\pmb u_k)}_{\text f(\cdot)进行预测} \, \underbrace{ p(\pmb x_{k-1} | \check{\pmb x}_0, \pmb u_{1:k-1}, \pmb z_{0:k-1})}_{先验} d \pmb x_{k-1} \end{split}
后验
p(xk∣xˇ0,u1:k,z0:k)=η似然
p(zk∣xk)先验
p(xk∣xˇ0,u1:k,z0:k−1)=ηp(zk∣xk)∫p(xk,xk−1∣xˇ0,u1:k,z0:k−1)dxk−1=ηp(zk∣xk)∫p(xk∣xk−1,xˇ0,u1:k,z1:k−1)p(xk−1∣xˇ0,u1:k,z0:k−1)dxk−1=ηh(⋅)进行更新
p(zk∣xk)∫f(⋅)进行预测
p(xk∣xk−1,uk)先验
p(xk−1∣xˇ0,u1:k−1,z0:k−1)dxk−1
广义高斯滤波
假设估计的状态为高斯分布时
从
k
−
1
k-1
k−1时刻高斯先验开始
p
(
x
k
−
1
∣
x
ˇ
0
,
u
1
:
k
−
1
,
z
0
:
k
−
1
)
∼
N
(
x
^
k
−
1
,
P
^
k
−
1
)
\begin{split} p(\pmb x_{k-1} | \check{\pmb x}_0, \pmb u_{1:k-1}, \pmb z_{0:k-1}) \sim \mathcal N(\hat{\pmb x}_{k-1}, \hat{\pmb P}_{k-1}) \end{split}
p(xk−1∣xˇ0,u1:k−1,z0:k−1)∼N(x^k−1,P^k−1)
预测步骤:利用最新的输入
u
k
\pmb u_k
uk,通过运动方程,得到
k
k
k时刻的高斯先验
p
(
x
k
∣
x
ˇ
0
,
u
1
:
k
,
z
0
:
k
−
1
)
∼
N
(
x
ˇ
k
,
P
ˇ
k
)
\begin{split} p(\pmb x_k | \check{\pmb x}_0, \pmb u_{1:k}, \pmb z_{0:k-1}) \sim \mathcal N(\check{\pmb x}_k, \check{\pmb P}_k) \end{split}
p(xk∣xˇ0,u1:k,z0:k−1)∼N(xˇk,Pˇk)
校正步骤:写出
k
k
k时刻的状态和最新测量的联合高斯分布
p
(
x
k
,
z
k
∣
x
ˇ
0
,
u
1
:
k
,
z
0
:
k
−
1
)
∼
N
(
[
μ
x
,
k
μ
z
,
k
]
,
[
Σ
x
x
,
k
Σ
x
z
,
k
Σ
z
x
,
k
Σ
z
z
,
k
]
)
\begin{split} p(\pmb x_k, \pmb z_k | \check{\pmb x}_0, \pmb u_{1:k}, \pmb z_{0:k-1}) \sim \mathcal N \left( \begin{bmatrix} \pmb \mu_{x,k} \\ \pmb \mu_{z,k}\end{bmatrix}, \begin{bmatrix} \pmb \Sigma_{xx,k} & \pmb \Sigma_{xz,k} \\ \pmb \Sigma_{zx,k} & \pmb \Sigma_{zz,k} \end{bmatrix} \right) \end{split}
p(xk,zk∣xˇ0,u1:k,z0:k−1)∼N([μx,kμz,k],[Σxx,kΣzx,kΣxz,kΣzz,k])
根据多元高斯概率的分解,得到
x
k
\pmb x_k
xk的条件概率密度(后验概率)
p
(
x
k
∣
x
ˇ
0
,
u
1
:
k
,
z
0
:
k
)
∼
N
(
μ
x
,
k
+
Σ
x
z
,
k
Σ
z
z
,
k
−
1
(
z
k
−
μ
z
,
k
)
⏟
x
^
k
,
Σ
x
x
,
k
−
Σ
x
z
,
k
Σ
z
z
,
k
−
1
Σ
z
x
,
k
⏟
P
k
^
)
\begin{split} p(\pmb x_k | \check{\pmb x}_0, \pmb u_{1:k}, \pmb z_{0:k}) \sim \mathcal N ( \underbrace{ \pmb \mu_{x,k} + \pmb \Sigma_{xz,k} \pmb \Sigma_{zz,k}^{-1}(\pmb z_k- \pmb \mu_{z,k}) }_{\hat{\pmb x}_k} , \underbrace{\pmb \Sigma_{xx,k} - \pmb \Sigma_{xz,k} \pmb \Sigma_{zz,k}^{-1} \pmb \Sigma_{zx,k} }_{\hat{\pmb P_k}} ) \end{split}
p(xk∣xˇ0,u1:k,z0:k)∼N(x^k
μx,k+Σxz,kΣzz,k−1(zk−μz,k),Pk^
Σxx,k−Σxz,kΣzz,k−1Σzx,k)
得出校正方程:
K
k
=
Σ
x
z
,
k
Σ
z
z
,
k
−
1
P
k
^
=
P
k
ˇ
−
K
k
Σ
x
z
,
k
T
x
^
k
=
x
ˇ
k
+
K
k
(
z
k
−
μ
z
,
k
)
\begin{split} \pmb K_k &= \pmb \Sigma_{xz,k} \pmb \Sigma_{zz,k}^{-1} \\ \hat{\pmb P_k} &= \check{\pmb P_k} - \pmb K_k \pmb \Sigma_{xz,k}^T \\ \hat{\pmb x}_k &= \check{\pmb x}_k + \pmb K_k(\pmb z_k- \pmb \mu_{z,k}) \end{split}
KkPk^x^k=Σxz,kΣzz,k−1=Pkˇ−KkΣxz,kT=xˇk+Kk(zk−μz,k)
其中,
μ
x
,
k
=
x
ˇ
k
\pmb \mu_{x,k} = \check{\pmb x}_k
μx,k=xˇk,
Σ
x
x
,
k
=
P
ˇ
k
\pmb \Sigma_{xx,k}=\check{\pmb P}_k
Σxx,k=Pˇk
卡尔曼滤波(KF)
运动方程: x k = A k − 1 x k − 1 + u k + w k 观测方程: z k = C k x k + v k \begin{split} 运动方程:\pmb x_k&=\pmb A_{k-1} \pmb x_{k-1} +\pmb u_k + \pmb w_k \\ 观测方程:\pmb z_k&=\pmb C_k\pmb x_k + \pmb v_k \\ \end{split} 运动方程:xk观测方程:zk=Ak−1xk−1+uk+wk=Ckxk+vk
- 过程噪声: w k ∈ R n ∼ N ( 0 , R k ) \pmb w_k \in \mathbb{R}^n \sim \mathcal N(\pmb 0,\pmb R_k) wk∈Rn∼N(0,Rk)
- 测量噪声: v k ∈ R m ∼ N ( 0 , Q k ) \pmb v_k \in \mathbb{R}^m \sim \mathcal N(\pmb 0,\pmb Q_k) vk∈Rm∼N(0,Qk)
预测步骤:
x
ˇ
k
=
A
k
−
1
x
^
k
−
1
+
u
k
P
ˇ
k
=
A
k
−
1
P
^
k
−
1
A
k
−
1
T
+
R
k
\begin{split} \check{\pmb x}_k &= \pmb A_{k-1} \hat{\pmb x}_{k-1} + \pmb u_k \\ \check{\pmb P}_k &= \pmb A_{k-1} \hat{\pmb P}_{k-1} \pmb A_{k-1}^T + \pmb R_k \end{split}
xˇkPˇk=Ak−1x^k−1+uk=Ak−1P^k−1Ak−1T+Rk
校正步骤:
联合高斯分布参数推导:
μ
z
,
k
=
E
[
z
k
]
=
E
[
C
k
x
k
+
v
k
]
=
C
k
x
ˇ
k
Σ
z
z
,
k
=
E
[
(
z
k
−
μ
z
,
k
)
(
z
k
−
μ
z
,
k
)
T
]
=
E
[
(
C
k
x
k
+
v
k
−
C
k
x
ˇ
k
)
(
C
k
x
k
+
v
k
−
C
k
x
ˇ
k
)
T
]
=
E
[
C
k
(
x
k
−
x
ˇ
k
)
+
v
k
)
(
C
k
(
x
k
−
x
ˇ
k
)
+
v
k
)
T
]
=
C
k
⋅
E
[
(
x
k
−
x
ˇ
k
)
(
x
k
−
x
ˇ
k
)
T
]
⋅
C
k
T
+
E
[
v
k
v
k
T
]
=
C
k
P
ˇ
k
C
k
T
+
Q
k
Σ
x
z
,
k
=
E
[
(
x
k
−
x
ˇ
k
)
(
z
k
−
μ
z
,
k
)
T
]
=
E
[
(
x
k
−
x
ˇ
k
)
(
C
k
(
x
k
−
x
ˇ
k
)
+
v
k
)
T
]
=
E
[
(
x
k
−
x
ˇ
k
)
(
x
k
−
x
ˇ
k
)
T
C
k
T
]
=
P
ˇ
k
C
k
T
Σ
z
x
,
k
=
Σ
x
z
,
k
T
=
C
k
P
ˇ
k
T
=
C
k
P
ˇ
k
\begin{split} \pmb \mu_{z,k} &= E[\pmb z_k] = E[\pmb C_k \pmb x_k + \pmb v_k] = \pmb C_k \check{\pmb x}_k \\ \pmb \Sigma_{zz,k} &= E[(\pmb z_k - \pmb \mu_{z,k})(\pmb z_k - \pmb \mu_{z,k})^T] \\ &= E[(\pmb C_k \pmb x_k + \pmb v_k - \pmb C_k \check{\pmb x}_k)(\pmb C_k \pmb x_k + \pmb v_k - \pmb C_k \check{\pmb x}_k)^T] \\ &= E[\pmb C_k (\pmb x_k-\check{\pmb x}_k)+ \pmb v_k)(\pmb C_k (\pmb x_k-\check{\pmb x}_k)+ \pmb v_k)^T] \\ &= \pmb C_k \cdot E[(\pmb x_k-\check{\pmb x}_k)(\pmb x_k-\check{\pmb x}_k)^T] \cdot \pmb C_k^T + E[\pmb v_k \pmb v_k^T] \\ &= \pmb C_k \check{\pmb P}_k \pmb C_k^T + \pmb Q_k \\ \pmb \Sigma_{xz,k} &= E[(\pmb x_k-\check{\pmb x}_k)(\pmb z_k - \pmb \mu_{z,k})^T] \\ &= E[(\pmb x_k-\check{\pmb x}_k)(\pmb C_k (\pmb x_k-\check{\pmb x}_k)+ \pmb v_k)^T] \\ &= E[(\pmb x_k-\check{\pmb x}_k) (\pmb x_k-\check{\pmb x}_k)^T \pmb C_k^T] \\ &= \check{\pmb P}_k \pmb C_k^T \\ \pmb \Sigma_{zx,k} &= \pmb \Sigma_{xz,k}^T \\ &= \pmb C_k \check{\pmb P}_k^T \\ &= \pmb C_k \check{\pmb P}_k \end{split}
μz,kΣzz,kΣxz,kΣzx,k=E[zk]=E[Ckxk+vk]=Ckxˇk=E[(zk−μz,k)(zk−μz,k)T]=E[(Ckxk+vk−Ckxˇk)(Ckxk+vk−Ckxˇk)T]=E[Ck(xk−xˇk)+vk)(Ck(xk−xˇk)+vk)T]=Ck⋅E[(xk−xˇk)(xk−xˇk)T]⋅CkT+E[vkvkT]=CkPˇkCkT+Qk=E[(xk−xˇk)(zk−μz,k)T]=E[(xk−xˇk)(Ck(xk−xˇk)+vk)T]=E[(xk−xˇk)(xk−xˇk)TCkT]=PˇkCkT=Σxz,kT=CkPˇkT=CkPˇk
带入广义高斯滤波得:
K
k
=
Σ
x
z
,
k
Σ
z
z
,
k
−
1
=
P
ˇ
k
C
k
T
(
C
k
P
ˇ
k
C
k
T
+
Q
k
)
−
1
P
k
^
=
P
k
ˇ
−
K
k
Σ
x
z
,
k
T
=
P
k
ˇ
−
K
k
C
k
P
ˇ
k
=
(
I
−
K
k
C
k
)
P
k
ˇ
x
^
k
=
x
ˇ
k
+
K
k
(
z
k
−
μ
z
,
k
)
\begin{split} \pmb K_k &= \pmb \Sigma_{xz,k} \pmb \Sigma_{zz,k}^{-1} \\ &= \check{\pmb P}_k \pmb C_k^T (\pmb C_k \check{\pmb P}_k \pmb C_k^T + \pmb Q_k)^{-1} \\ \hat{\pmb P_k} &= \check{\pmb P_k} - \pmb K_k \pmb \Sigma_{xz,k}^T \\ &= \check{\pmb P_k} - \pmb K_k \pmb C_k \check{\pmb P}_k \\ &= (\pmb I - \pmb K_k \pmb C_k) \check{\pmb P_k} \\ \hat{\pmb x}_k &= \check{\pmb x}_k + \pmb K_k(\pmb z_k- \pmb \mu_{z,k}) \\ \end{split}
KkPk^x^k=Σxz,kΣzz,k−1=PˇkCkT(CkPˇkCkT+Qk)−1=Pkˇ−KkΣxz,kT=Pkˇ−KkCkPˇk=(I−KkCk)Pkˇ=xˇk+Kk(zk−μz,k)
扩展卡尔曼滤波(EKF)
对运动方程和观测方程进行线性化,即进行一阶泰勒展开
运动方程:
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
=
f
(
x
^
k
−
1
,
u
k
)
+
∂
f
∂
x
k
−
1
∣
x
^
k
−
1
(
x
k
−
1
−
x
^
k
−
1
)
+
w
k
=
f
(
x
^
k
−
1
,
u
k
)
+
F
k
−
1
(
x
k
−
1
−
x
^
k
−
1
)
+
w
k
观测方程:
z
k
=
h
(
x
k
)
+
v
k
=
h
(
x
ˇ
k
−
1
)
+
∂
h
x
k
∣
x
ˇ
k
−
1
(
x
k
−
x
ˇ
k
−
1
)
+
v
k
=
h
(
x
ˇ
k
−
1
)
+
H
k
(
x
k
−
x
ˇ
k
−
1
)
+
v
k
\begin{split} 运动方程:\pmb x_k &=\pmb f(\pmb x_{k-1},\pmb u_k) + \pmb w_k \\ &= \pmb f(\hat{\pmb x}_{k-1}, \pmb u_k) + \frac{\partial \pmb f}{\partial \pmb x_{k-1}}|_{\hat{\pmb x}_{k-1}} (\pmb x_{k-1} - \hat{\pmb x}_{k-1}) + \pmb w_k \\ &= \pmb f(\hat{\pmb x}_{k-1}, \pmb u_k) + \pmb F_{k-1} (\pmb x_{k-1} - \hat{\pmb x}_{k-1}) + \pmb w_k \\ 观测方程:\pmb z_k &=\pmb h(\pmb x_k) + \pmb v_k \\ &= \pmb h(\check{\pmb x}_{k-1}) + \frac{\partial \pmb h}{\pmb x_k} |_{\check{\pmb x}_{k-1}} (\pmb x_k - \check{\pmb x}_{k-1}) + \pmb v_k \\ &= \pmb h(\check{\pmb x}_{k-1}) + \pmb H_k (\pmb x_k - \check{\pmb x}_{k-1}) + \pmb v_k \\ \end{split}
运动方程:xk观测方程:zk=f(xk−1,uk)+wk=f(x^k−1,uk)+∂xk−1∂f∣x^k−1(xk−1−x^k−1)+wk=f(x^k−1,uk)+Fk−1(xk−1−x^k−1)+wk=h(xk)+vk=h(xˇk−1)+xk∂h∣xˇk−1(xk−xˇk−1)+vk=h(xˇk−1)+Hk(xk−xˇk−1)+vk
- 过程噪声: w k ∈ R n ∼ N ( 0 , R k ) \pmb w_k \in \mathbb{R}^n \sim \mathcal N(\pmb 0,\pmb R_k) wk∈Rn∼N(0,Rk)
- 测量噪声: v k ∈ R m ∼ N ( 0 , Q k ) \pmb v_k \in \mathbb{R}^m \sim \mathcal N(\pmb 0,\pmb Q_k) vk∈Rm∼N(0,Qk)
预测步骤:
x
ˇ
k
=
f
(
x
^
k
−
1
,
u
k
)
P
ˇ
k
=
F
k
−
1
P
^
k
−
1
F
k
−
1
T
+
R
k
\begin{split} \check{\pmb x}_k &= \pmb f(\hat{\pmb x}_{k-1}, \pmb u_k) \\ \check{\pmb P}_k &= \pmb F_{k-1} \hat{\pmb P}_{k-1} \pmb F_{k-1}^T + \pmb R_k \\ \end{split}
xˇkPˇk=f(x^k−1,uk)=Fk−1P^k−1Fk−1T+Rk
校正步骤:
联合高斯分布推导:
μ
z
,
k
=
E
[
z
k
]
=
E
[
h
(
x
ˇ
k
−
1
)
+
H
k
(
x
k
−
x
ˇ
k
−
1
)
+
v
k
]
=
h
(
x
ˇ
k
−
1
)
Σ
z
z
,
k
=
E
[
(
z
k
−
μ
z
,
k
)
(
z
k
−
μ
z
,
k
)
T
]
=
E
[
(
h
(
x
ˇ
k
−
1
)
+
H
k
(
x
k
−
x
ˇ
k
−
1
)
+
v
k
−
h
(
x
ˇ
k
−
1
)
)
(
h
(
x
ˇ
k
−
1
)
+
H
k
(
x
k
−
x
ˇ
k
−
1
)
+
v
k
−
h
(
x
ˇ
k
−
1
)
)
T
]
=
E
[
(
H
k
(
x
k
−
x
ˇ
k
−
1
)
+
v
k
)
(
H
k
(
x
k
−
x
ˇ
k
−
1
)
+
v
k
)
T
]
=
H
k
P
ˇ
k
H
k
T
+
Q
k
Σ
x
z
,
k
=
E
[
(
x
k
−
x
ˇ
k
)
(
z
k
−
μ
z
,
k
)
T
]
=
E
[
(
x
k
−
x
ˇ
k
)
(
h
(
x
ˇ
k
−
1
)
+
H
k
(
x
k
−
x
ˇ
k
−
1
)
+
v
k
−
h
(
x
ˇ
k
−
1
)
)
T
]
=
E
[
(
x
k
−
x
ˇ
k
)
(
H
k
(
x
k
−
x
ˇ
k
−
1
)
+
v
k
)
T
]
=
P
ˇ
k
H
k
T
Σ
z
x
,
k
=
Σ
x
z
,
k
T
=
H
k
P
ˇ
k
T
=
H
k
P
ˇ
k
\begin{split} \pmb \mu_{z,k} &= E[\pmb z_k] \\ &= E[\pmb h(\check{\pmb x}_{k-1}) + \pmb H_k (\pmb x_k - \check{\pmb x}_{k-1}) + \pmb v_k] \\ &= \pmb h(\check{\pmb x}_{k-1}) \\ \pmb \Sigma_{zz,k} &= E[(\pmb z_k - \pmb \mu_{z,k})(\pmb z_k - \pmb \mu_{z,k})^T] \\ &= E[(\pmb h(\check{\pmb x}_{k-1}) + \pmb H_k (\pmb x_k - \check{\pmb x}_{k-1}) + \pmb v_k - \pmb h(\check{\pmb x}_{k-1})) \\& \qquad \space (\pmb h(\check{\pmb x}_{k-1}) + \pmb H_k (\pmb x_k - \check{\pmb x}_{k-1}) + \pmb v_k - \pmb h(\check{\pmb x}_{k-1}))^T] \\ &= E[(\pmb H_k (\pmb x_k - \check{\pmb x}_{k-1}) + \pmb v_k) (\pmb H_k (\pmb x_k - \check{\pmb x}_{k-1}) + \pmb v_k)^T] \\ &= \pmb H_k \check{\pmb P}_k \pmb H_k^T + \pmb Q_k \\ \pmb \Sigma_{xz,k} &= E[(\pmb x_k-\check{\pmb x}_k)(\pmb z_k - \pmb \mu_{z,k})^T] \\ &= E[(\pmb x_k-\check{\pmb x}_k)(\pmb h(\check{\pmb x}_{k-1}) + \pmb H_k (\pmb x_k - \check{\pmb x}_{k-1}) + \pmb v_k - \pmb h(\check{\pmb x}_{k-1}))^T] \\ &= E[(\pmb x_k-\check{\pmb x}_k) (\pmb H_k (\pmb x_k - \check{\pmb x}_{k-1}) + \pmb v_k)^T] \\ &= \check{\pmb P}_k \pmb H_k^T \\ \pmb \Sigma_{zx,k} &= \pmb \Sigma_{xz,k}^T \\ &= \pmb H_k \check{\pmb P}_k^T \\ &= \pmb H_k \check{\pmb P}_k \end{split}
μz,kΣzz,kΣxz,kΣzx,k=E[zk]=E[h(xˇk−1)+Hk(xk−xˇk−1)+vk]=h(xˇk−1)=E[(zk−μz,k)(zk−μz,k)T]=E[(h(xˇk−1)+Hk(xk−xˇk−1)+vk−h(xˇk−1)) (h(xˇk−1)+Hk(xk−xˇk−1)+vk−h(xˇk−1))T]=E[(Hk(xk−xˇk−1)+vk)(Hk(xk−xˇk−1)+vk)T]=HkPˇkHkT+Qk=E[(xk−xˇk)(zk−μz,k)T]=E[(xk−xˇk)(h(xˇk−1)+Hk(xk−xˇk−1)+vk−h(xˇk−1))T]=E[(xk−xˇk)(Hk(xk−xˇk−1)+vk)T]=PˇkHkT=Σxz,kT=HkPˇkT=HkPˇk
带入广义高斯滤波器得:
K
k
=
Σ
x
z
,
k
Σ
z
z
,
k
−
1
=
P
ˇ
k
H
k
T
(
H
k
P
ˇ
k
H
k
T
+
Q
k
)
−
1
P
k
^
=
P
k
ˇ
−
K
k
Σ
x
z
,
k
T
=
P
k
ˇ
−
K
k
H
k
P
ˇ
k
=
(
I
−
K
k
H
k
)
P
k
ˇ
x
^
k
=
x
ˇ
k
+
K
k
(
z
k
−
μ
z
,
k
)
=
x
ˇ
k
+
K
k
(
z
k
−
h
(
x
ˇ
k
−
1
)
)
\begin{split} \pmb K_k &= \pmb \Sigma_{xz,k} \pmb \Sigma_{zz,k}^{-1} \\ &= \check{\pmb P}_k \pmb H_k^T (\pmb H_k \check{\pmb P}_k \pmb H_k^T + \pmb Q_k)^{-1} \\ \hat{\pmb P_k} &= \check{\pmb P_k} - \pmb K_k \pmb \Sigma_{xz,k}^T \\ &= \check{\pmb P_k} - \pmb K_k \pmb H_k \check{\pmb P}_k \\ &= (\pmb I - \pmb K_k \pmb H_k) \check{\pmb P_k} \\ \hat{\pmb x}_k &= \check{\pmb x}_k + \pmb K_k(\pmb z_k- \pmb \mu_{z,k}) \\ &= \check{\pmb x}_k + \pmb K_k(\pmb z_k - \pmb h(\check{\pmb x}_{k-1})) \end{split}
KkPk^x^k=Σxz,kΣzz,k−1=PˇkHkT(HkPˇkHkT+Qk)−1=Pkˇ−KkΣxz,kT=Pkˇ−KkHkPˇk=(I−KkHk)Pkˇ=xˇk+Kk(zk−μz,k)=xˇk+Kk(zk−h(xˇk−1))
误差卡尔曼滤波(ESKF)
将真值(true)状态变量分解为名义(nominal)状态变量和误差(error)状态变量的组合: x t = x ⊕ δ x \pmb x_t = \pmb x \oplus \delta \pmb x xt=x⊕δx。书写简单起见,无特殊描述时,下文统一使用一般的 + / − +/- +/−符号。
核心思想是将名义状态视为大信号(可非线性积分),将误差状态视为小信号(因此可线性积分,适用于线性高斯滤波)。
重写运动方程,将输入噪声项叠加到输入
u
k
\pmb u_k
uk上
x
t
,
k
=
f
(
x
t
,
k
−
1
,
u
t
,
k
)
x
k
+
δ
x
k
=
f
(
x
k
−
1
+
δ
x
k
−
1
,
u
k
+
δ
u
k
)
=
f
(
x
k
−
1
,
u
k
)
+
∂
f
∂
x
∣
x
k
−
1
⋅
δ
x
k
−
1
+
∂
f
∂
u
∣
u
k
⋅
δ
u
k
δ
x
k
=
F
k
−
1
⋅
δ
x
k
−
1
+
G
k
⋅
δ
u
k
\begin{split} \pmb x_{t,k} &= \pmb f(\pmb x_{t,k-1},\pmb u_{t,k}) \\ \pmb x_{k} + \delta \pmb x_k &= \pmb f(\pmb x_{k-1} + \delta \pmb x_{k-1},\pmb u_{k} + \delta \pmb u_k) \\ &= \pmb f(\pmb x_{k-1},\pmb u_{k}) + \frac{\partial \pmb f}{\partial \pmb x}|_{\pmb x_{k-1}} \cdot \delta \pmb x_{k-1} + \frac{\partial \pmb f}{\partial \pmb u}|_{\pmb u_{k}} \cdot \delta \pmb u_k \\ \delta \pmb x_k &= \pmb F_{k-1} \cdot \delta \pmb x_{k-1} + \pmb G_k \cdot \delta \pmb u_k \end{split}
xt,kxk+δxkδxk=f(xt,k−1,ut,k)=f(xk−1+δxk−1,uk+δuk)=f(xk−1,uk)+∂x∂f∣xk−1⋅δxk−1+∂u∂f∣uk⋅δuk=Fk−1⋅δxk−1+Gk⋅δuk
- 过程噪声: G k ⋅ δ u k = w k ∼ N ( 0 , R k ) \pmb G_k \cdot \delta \pmb u_k = \pmb w_k \sim \mathcal N(\pmb 0, \pmb R_k) Gk⋅δuk=wk∼N(0,Rk)
δ x k = F k − 1 ⋅ δ x k − 1 + w k \delta \pmb x_k = \pmb F_{k-1} \cdot \delta \pmb x_{k-1} + \pmb w_k δxk=Fk−1⋅δxk−1+wk
- 初始状态: δ x ^ 0 ∼ N ( 0 , P ^ 0 ) \delta \hat{\pmb x}_0 \sim \mathcal N(\pmb 0, \hat{\pmb P}_0) δx^0∼N(0,P^0)
重写观测方程
z
k
=
h
(
x
t
,
k
)
+
v
k
=
h
(
x
k
+
δ
x
k
)
+
v
k
=
h
(
x
k
)
+
∂
h
∂
δ
x
k
∣
x
k
⋅
δ
x
k
+
v
k
=
h
(
x
k
)
+
H
k
⋅
δ
x
k
+
v
k
\begin{split} \pmb z_k &=\pmb h(\pmb x_{t,k})+\pmb v_k \\ &= \pmb h(\pmb x_k + \delta \pmb x_k) + \pmb v_k \\ &= \pmb h(\pmb x_k) + \frac{\partial \pmb h}{\partial \delta \pmb x_k}|_{\pmb x_k} \cdot \delta \pmb x_k + \pmb v_k \\ &= \pmb h(\pmb x_k) + \pmb H_k \cdot \delta \pmb x_k + \pmb v_k \\ \end{split}
zk=h(xt,k)+vk=h(xk+δxk)+vk=h(xk)+∂δxk∂h∣xk⋅δxk+vk=h(xk)+Hk⋅δxk+vk
- 测量噪声: v k ∈ R m ∼ N ( 0 , Q k ) \pmb v_k \in \mathbb{R}^m \sim \mathcal N(\pmb 0,\pmb Q_k) vk∈Rm∼N(0,Qk)
预测步骤:
x
k
=
f
(
x
k
−
1
,
u
k
)
δ
x
ˇ
k
=
F
k
−
1
⋅
δ
x
^
k
−
1
P
ˇ
k
=
F
k
−
1
P
^
k
−
1
F
k
−
1
T
+
R
k
\begin{split} \pmb x_k &= \pmb f(\pmb x_{k-1}, \pmb u_k) \\ \delta \check{\pmb x}_k &= \pmb F_{k-1} \cdot \delta \hat{\pmb x}_{k-1}\\ \check{\pmb P}_k &= \pmb F_{k-1} \hat{\pmb P}_{k-1} \pmb F_{k-1}^T + \pmb R_k \\ \end{split}
xkδxˇkPˇk=f(xk−1,uk)=Fk−1⋅δx^k−1=Fk−1P^k−1Fk−1T+Rk
说明:
δ
x
ˇ
k
=
F
k
−
1
⋅
δ
x
^
k
−
1
\delta \check{\pmb x}_k = \pmb F_{k-1} \cdot \delta \hat{\pmb x}_{k-1}
δxˇk=Fk−1⋅δx^k−1在此更新意义不大,因每次更新后,会重置
δ
x
k
=
0
\delta {\pmb x}_k = \pmb 0
δxk=0
校正步骤:
联合高斯分布推导
μ
x
,
k
=
E
[
δ
x
k
]
=
0
Σ
x
x
,
k
=
E
[
δ
x
k
⋅
δ
x
k
T
]
=
P
ˇ
k
μ
z
,
k
=
E
[
h
(
x
k
)
+
H
k
⋅
δ
x
k
+
v
k
]
=
h
(
x
k
)
Σ
z
z
,
k
=
E
[
(
z
k
−
μ
z
,
k
)
(
z
k
−
μ
z
,
k
)
T
]
=
E
[
(
h
(
x
k
)
+
H
k
⋅
δ
x
k
+
v
k
−
h
(
x
k
)
(
h
(
x
k
)
+
H
k
⋅
δ
x
k
+
v
k
−
h
(
x
k
)
T
]
=
E
[
(
H
k
⋅
δ
x
k
+
v
k
)
(
H
k
⋅
δ
x
k
+
v
k
)
T
]
=
H
k
P
ˇ
k
H
k
T
+
Q
k
Σ
x
z
,
k
=
E
[
(
δ
x
k
−
δ
x
ˇ
k
)
(
z
k
−
μ
z
,
k
)
T
]
=
E
[
(
δ
x
k
)
(
H
k
⋅
δ
x
k
+
v
k
)
T
]
=
P
ˇ
k
H
k
T
Σ
z
x
,
k
=
Σ
x
z
,
k
T
=
H
k
P
ˇ
k
T
=
H
k
P
ˇ
k
\begin{split} \pmb \mu_{x,k} &= E[\delta \pmb x_k] = \pmb 0 \\ \pmb \Sigma_{xx,k} &= E[\delta \pmb x_k \cdot \delta \pmb x_k^T] = \check{\pmb P}_k \\ \pmb \mu_{z,k} &= E[\pmb h(\pmb x_k) + \pmb H_k \cdot \delta \pmb x_k + \pmb v_k] \\ &= \pmb h(\pmb x_k) \\ \pmb \Sigma_{zz,k} &= E[(\pmb z_k - \pmb \mu_{z,k})(\pmb z_k - \pmb \mu_{z,k})^T] \\ &= E[(\pmb h(\pmb x_k) + \pmb H_k \cdot \delta \pmb x_k + \pmb v_k - \pmb h(\pmb x_k)(\pmb h(\pmb x_k) + \pmb H_k \cdot \delta \pmb x_k + \pmb v_k - \pmb h(\pmb x_k)^T] \\ &= E[(\pmb H_k \cdot \delta \pmb x_k + \pmb v_k)(\pmb H_k \cdot \delta \pmb x_k + \pmb v_k)^T] \\ &= \pmb H_k \check{\pmb P}_k \pmb H_k^T + \pmb Q_k \\ \pmb \Sigma_{xz,k} &= E[(\delta \pmb x_k - \delta \check{\pmb x}_k)(\pmb z_k - \pmb \mu_{z,k})^T] \\ &= E[(\delta \pmb x_k) (\pmb H_k \cdot \delta \pmb x_k + \pmb v_k)^T] \\ &= \check{\pmb P}_k \pmb H_k^T \\ \pmb \Sigma_{zx,k} &= \pmb \Sigma_{xz,k}^T \\ &= \pmb H_k \check{\pmb P}_k^T \\ &= \pmb H_k \check{\pmb P}_k \end{split}
μx,kΣxx,kμz,kΣzz,kΣxz,kΣzx,k=E[δxk]=0=E[δxk⋅δxkT]=Pˇk=E[h(xk)+Hk⋅δxk+vk]=h(xk)=E[(zk−μz,k)(zk−μz,k)T]=E[(h(xk)+Hk⋅δxk+vk−h(xk)(h(xk)+Hk⋅δxk+vk−h(xk)T]=E[(Hk⋅δxk+vk)(Hk⋅δxk+vk)T]=HkPˇkHkT+Qk=E[(δxk−δxˇk)(zk−μz,k)T]=E[(δxk)(Hk⋅δxk+vk)T]=PˇkHkT=Σxz,kT=HkPˇkT=HkPˇk
带入广义高斯滤波得:
K
k
=
P
ˇ
k
H
k
T
(
H
k
P
ˇ
k
H
k
T
+
Q
k
)
−
1
P
k
^
=
(
I
−
K
k
H
k
)
P
k
ˇ
δ
x
^
k
=
K
k
(
z
k
−
μ
z
,
k
)
=
K
k
(
z
k
−
h
(
x
k
)
)
\begin{split} \pmb K_k &= \check{\pmb P}_k \pmb H_k^T (\pmb H_k \check{\pmb P}_k \pmb H_k^T + \pmb Q_k)^{-1} \\ \hat{\pmb P_k} &= (\pmb I - \pmb K_k \pmb H_k) \check{\pmb P_k} \\ \delta \hat{\pmb x}_k &= \pmb K_k(\pmb z_k- \pmb \mu_{z,k}) \\ &= \pmb K_k(\pmb z_k - \pmb h(\pmb x_k)) \end{split}
KkPk^δx^k=PˇkHkT(HkPˇkHkT+Qk)−1=(I−KkHk)Pkˇ=Kk(zk−μz,k)=Kk(zk−h(xk))
误差重置:
更新完成后,将观测误差 δ x k \delta \pmb x_k δxk更新到名义状态变量中,保持误差状态变量均值为零。
名义状态变量更新:
x
^
k
=
x
k
+
δ
x
^
k
\begin{split} \hat{\pmb x}_k = \pmb x_k + \delta \hat{\pmb x}_k \end{split}
x^k=xk+δx^k
状态误差重置:
δ
x
k
←
g
(
δ
x
k
)
=
δ
x
k
⊖
δ
x
^
k
P
←
J
P
J
T
J
=
∂
g
∂
δ
x
∣
δ
x
^
k
\begin{split} \delta \pmb x_k &\leftarrow \pmb g(\delta \pmb x_k) = \delta \pmb x_k \ominus \delta \hat{\pmb x}_k \\ \pmb P &\leftarrow \pmb J \pmb P \pmb J^T \\ \pmb J &= \frac{\partial \pmb g}{\partial \delta \pmb x} |_{\delta \hat{x}_k} \end{split}
δxkPJ←g(δxk)=δxk⊖δx^k←JPJT=∂δx∂g∣δx^k
重置前:
δ
x
k
∼
N
(
δ
x
^
k
,
P
^
k
)
\delta \pmb x_k \sim \mathcal N(\delta \hat{\pmb x}_k,\hat{\pmb P}_k)
δxk∼N(δx^k,P^k)描述的是关于
x
k
\pmb x_k
xk切空间的分布
重置后: δ x k ∼ N ( 0 , P k ) \delta \pmb x_k \sim \mathcal N(\pmb 0,\pmb P_k) δxk∼N(0,Pk)描述的是关于 x k + δ x k \pmb x_k + \delta \pmb x_k xk+δxk切空间的分布
对于欧拉空间 J \pmb J J为单位阵,但是对于带旋转的状态变量,切空间零点发生改变。Jacobian推导如下:
-
旧的角度误差 δ θ \delta \pmb \theta δθ及其对应的四元数 δ q \delta \pmb q δq,观测误差 δ θ ^ \delta \hat{\pmb \theta} δθ^
-
新的角度误差 δ θ + \delta \pmb \theta^+ δθ+及其对应的四元数 δ q + \delta \pmb q^+ δq+
-
新的名义旋转变量 q + \pmb q^+ q+
-
旋转的真值不变:
q t = q + ⊗ δ q + = q ⊗ δ q \begin{split} \pmb q_t = \pmb q^+ \otimes \delta \pmb q^+ = \pmb q \otimes \delta \pmb q \end{split} qt=q+⊗δq+=q⊗δq -
观察误差注入到名义状态变量
q + = q ⊗ δ q ^ \begin{split} \pmb q^+ = \pmb q \otimes \delta \hat{\pmb q} \end{split} q+=q⊗δq^ -
联合上述两式得
δ q + = δ q ^ ∗ ⊗ δ q = [ δ q ^ ∗ ] L ⋅ δ q ≈ [ 1 − 1 2 δ θ ^ ] L ⋅ δ q [ 1 1 2 δ θ + ] ≈ [ 1 1 2 δ θ ^ T − 1 2 δ θ ^ I − [ 1 2 δ θ ^ ] × ] ⋅ [ 1 1 2 δ θ ] δ θ + ≈ − δ θ ^ + ( I − [ 1 2 δ θ ^ ] × ) δ θ \begin{split} \delta \pmb q^+ &= \delta \hat{\pmb q}^* \otimes \delta \pmb q \\ &= [\delta \hat{\pmb q}^*]_L \cdot \delta \pmb q \\ &\approx \begin{bmatrix} 1 \\ -\frac12 \delta \hat{\pmb \theta} \end{bmatrix}_L \cdot \delta \pmb q \\ \begin{bmatrix} 1 \\ \frac12 \delta \pmb \theta^+ \end{bmatrix} &\approx \begin{bmatrix} 1 & \frac12 \delta \hat{\pmb \theta}^T \\ -\frac12 \delta \hat{\pmb \theta} & \pmb I - \left[\frac12 \delta \hat{\pmb \theta} \right]_\times \end{bmatrix} \cdot \begin{bmatrix} 1 \\ \frac12 \delta \pmb \theta \end{bmatrix} \\ \delta \pmb \theta^+ &\approx - \delta \hat{\pmb \theta} + \left( \pmb I - \left[\frac12 \delta \hat{\pmb \theta} \right]_\times \right) \delta \pmb \theta \\ \end{split} δq+[121δθ+]δθ+=δq^∗⊗δq=[δq^∗]L⋅δq≈[1−21δθ^]L⋅δq≈ 1−21δθ^21δθ^TI−[21δθ^]× ⋅[121δθ]≈−δθ^+(I−[21δθ^]×)δθ
得到:
∂ δ θ + ∂ δ θ = I − [ 1 2 δ θ ^ ] × \begin{split} \frac{\partial \delta \pmb \theta^+}{\partial \delta \pmb \theta} = \pmb I - \left[\frac12 \delta \hat{\pmb \theta} \right]_\times \end{split} ∂δθ∂δθ+=I−[21δθ^]×