- True-state 真值状态 x \bold{x} x
- Nominal-state 名义状态 x ˉ \bar{\bold{x}} xˉ
- Error-state 误差状态
δ
x
\delta \bold{x}
δx
x = x ˉ + δ x \bold{x}=\bar{\bold{x}}+\delta \bold{x} x=xˉ+δx
- 真值状态被表达成名义状态和误差状态的组合(线性相加、四元数乘法或矩阵乘法)
- 名义状态不考虑噪声项和其他可能的建模误差
连续时间模型
假设系统模型方程:
x
˙
(
t
)
=
f
(
x
(
t
)
,
u
(
t
)
,
w
(
t
)
)
\dot{\bold{x}}\left( t\right) =f\left( \bold{x}\left( t\right) ,\bold{u}\left( t\right) ,\bold{w}\left( t\right) \right)
x˙(t)=f(x(t),u(t),w(t))
则按照名义状态的定义，有：
x
ˉ
˙
(
t
)
=
f
(
x
ˉ
(
t
)
,
u
(
t
)
,
0
)
\dot{\bar{\bold{x}}}\left( t\right) =f\left( \bar{\bold{x}}\left( t\right) ,\bold{u}\left( t\right) ,\bold{0}\right)
xˉ˙(t)=f(xˉ(t),u(t),0)
x
˙
(
t
)
=
(
x
ˉ
(
t
)
+
δ
x
)
˙
=
x
ˉ
˙
(
t
)
+
δ
x
˙
=
f
(
x
ˉ
(
t
)
+
δ
x
(
t
)
,
u
(
t
)
,
w
(
t
)
)
\begin{aligned} \dot{\bold{x}}(t)&=\dot{(\bar{\bold{x}}\left( t\right) +\delta \bold{x})}=\dot{\bar{\bold{x}}}\left( t\right) +\dot{\delta \bold{x}} \\&=f\left( \bar{\bold{x}}\left( t\right) +\delta \bold{x}\left( t\right) ,\bold{u}\left( t\right) ,\bold{w} \left( t\right) \right) \end{aligned}
x˙(t)=(xˉ(t)+δx)˙=xˉ˙(t)+δx˙=f(xˉ(t)+δx(t),u(t),w(t))
δ
x
˙
(
t
)
=
x
˙
(
t
)
−
x
ˉ
˙
(
t
)
=
f
(
x
ˉ
(
t
)
+
δ
x
(
t
)
,
u
(
t
)
,
w
(
t
)
)
−
f
(
x
ˉ
(
t
)
,
u
(
t
)
,
0
)
=
∂
f
(
x
ˉ
(
t
)
,
u
(
t
)
,
0
)
∂
δ
x
δ
x
(
t
)
+
1
2
!
∂
2
f
(
x
ˉ
(
t
)
,
u
(
t
)
,
0
)
∂
δ
x
2
δ
x
(
t
)
2
+
⋯
+
∂
f
(
x
ˉ
(
t
)
,
u
(
t
)
,
0
)
∂
w
w
(
t
)
+
1
2
!
∂
2
f
(
x
ˉ
(
t
)
,
u
(
t
)
,
0
)
∂
w
2
w
(
t
)
2
+
⋯
=
∂
f
(
x
ˉ
(
t
)
,
u
(
t
)
,
0
)
∂
δ
x
δ
x
(
t
)
+
O
(
δ
x
(
t
)
)
+
∂
f
(
x
ˉ
(
t
)
,
u
(
t
)
,
0
)
∂
w
w
(
t
)
+
O
(
w
(
t
)
)
\begin{aligned} \dot{\delta \bold{x}}(t)&=\dot{\bold{x}}\left( t\right) - \dot{\bar{\bold{x}}}\left( t\right) =f\left( \bar{\bold{x}}\left( t\right) +\delta \bold{x}\left( t\right) ,\bold{u}\left( t\right) ,\bold{w} \left( t\right) \right) - f\left( \bar{\bold{x}}\left( t\right) ,\bold{u}\left( t\right) ,\bold{0}\right) \\ &=\dfrac{\partial f(\bar{\bold{x}}(t),\bold{u}(t),\bold{0})}{\partial \delta \bold{x}}\delta \bold{x} \left( t\right)+\dfrac{1}{2!}\dfrac{\partial^2 f(\bar{\bold{x}}(t),\bold{u}(t),\bold{0})}{\partial \delta \bold{x}^2}\delta \bold{x} \left( t\right)^2+\cdots \\&\quad+\dfrac{\partial f(\bar{\bold{x}}(t),\bold{u}(t),\bold{0})}{\partial \bold{w}}\bold{w} \left( t\right)+\dfrac{1}{2!}\dfrac{\partial^2 f(\bar{\bold{x}}(t),\bold{u}(t),\bold{0})}{\partial \bold{w}^2}\bold{w} \left( t\right)^2+\cdots \\ &=\dfrac{\partial f(\bar{\bold{x}}(t),\bold{u}(t),\bold{0})}{\partial \delta \bold{x}}\delta \bold{x} \left( t\right) +O\left( \delta \bold{x}\left( t\right) \right) +\dfrac{\partial f(\bar{\bold{x}}(t),\bold{u}(t),\bold{0})}{\partial \bold{w} }\bold{w}\left( t\right) +O\left( \bold{w} \left( t\right) \right) \end{aligned}
δx˙(t)=x˙(t)−xˉ˙(t)=f(xˉ(t)+δx(t),u(t),w(t))−f(xˉ(t),u(t),0)=∂δx∂f(xˉ(t),u(t),0)δx(t)+2!1∂δx2∂2f(xˉ(t),u(t),0)δx(t)2+⋯+∂w∂f(xˉ(t),u(t),0)w(t)+2!1∂w2∂2f(xˉ(t),u(t),0)w(t)2+⋯=∂δx∂f(xˉ(t),u(t),0)δx(t)+O(δx(t))+∂w∂f(xˉ(t),u(t),0)w(t)+O(w(t))
记
F
(
t
)
=
∂
f
(
x
ˉ
(
t
)
,
u
(
t
)
,
0
)
∂
δ
x
,
L
(
t
)
=
∂
f
(
x
ˉ
(
t
)
,
u
(
t
)
,
0
)
∂
w
\bold{F}(t)=\dfrac{\partial f(\bar{\bold{x}}(t),\bold{u}(t),\bold{0})}{\partial \delta \bold{x}},\ \bold{L}(t)=\dfrac{\partial f(\bar{\bold{x}}(t),\bold{u}(t),\bold{0})}{\partial \bold{w} }
F(t)=∂δx∂f(xˉ(t),u(t),0), L(t)=∂w∂f(xˉ(t),u(t),0)，忽略噪声
w
\bold{w}
w的高阶项，则
δ
x
˙
(
t
)
=
F
(
t
)
δ
x
(
t
)
+
L
(
t
)
w
(
t
)
+
O
(
δ
x
(
t
)
)
\dot{\delta \bold{x}}(t)=\bold{F}(t)\delta \bold{x}(t)+\bold{L}(t)\bold{w}(t)+O\left( \delta \bold{x}\left( t\right)\right)
δx˙(t)=F(t)δx(t)+L(t)w(t)+O(δx(t))
误差状态方程的离散化：
δ
x
(
t
+
Δ
t
)
=
δ
x
(
t
)
+
δ
x
˙
(
t
)
Δ
t
=
δ
x
(
t
)
+
F
(
t
)
δ
x
(
t
)
Δ
t
+
O
(
δ
x
(
t
)
)
Δ
t
+
L
(
t
)
w
(
t
)
Δ
t
=
(
I
+
F
(
t
)
Δ
t
)
δ
x
(
t
)
+
O
(
δ
x
(
t
)
)
Δ
t
+
L
(
t
)
w
(
t
)
Δ
t
=
Φ
(
t
)
δ
x
(
t
)
+
O
(
δ
x
(
t
)
)
Δ
t
+
L
(
t
)
w
(
t
)
Δ
t
\begin{aligned} \delta \bold{x}\left( t+\Delta t\right) &=\delta \bold{x}\left( t\right) + \dot{\delta \bold{x}}\left( t\right) \Delta t\\ &=\delta \bold{x}\left( t\right) +\bold{F}\left( t\right) \delta \bold{x}\left( t\right) \Delta t+O\left( \delta \bold{x}\left( t\right) \right) \Delta t+\bold{L}\left( t\right) \bold{w}\left( t\right) \Delta t\\ &=\left( I+\bold{F}\left( t\right) \Delta t\right) \delta \bold{x}\left( t\right) +O\left( \delta \bold{x}\left( t\right) \right) \Delta t+\bold{L}\left( t\right) \bold{w}\left( t\right) \Delta t\\ &=\Phi \left( t\right) \delta \bold{x}\left( t\right) +O\left( \delta \bold{x}\left( t\right) \right) \Delta t+\bold{L}\left( t\right) \bold{w}\left( t\right) \Delta t \end{aligned}
δx(t+Δt)=δx(t)+δx˙(t)Δt=δx(t)+F(t)δx(t)Δt+O(δx(t))Δt+L(t)w(t)Δt=(I+F(t)Δt)δx(t)+O(δx(t))Δt+L(t)w(t)Δt=Φ(t)δx(t)+O(δx(t))Δt+L(t)w(t)Δt
在ESKF中，
Φ
(
t
)
\Phi(t)
Φ(t)被称作State Transition Matrix。ESKF的Predict步骤作近似
δ
x
(
t
+
Δ
t
)
≃
Φ
(
t
)
δ
x
(
t
)
\delta \bold{x}\left( t+\Delta t\right)\simeq \Phi(t)\delta \bold{x}\left( t\right)
δx(t+Δt)≃Φ(t)δx(t) ，剩下的由于非线性导致的误差项
O
(
δ
x
(
t
)
)
Δ
t
+
L
(
t
)
w
(
t
)
Δ
t
O\left( \delta \bold{x}\left( t\right) \right) \Delta t+\bold{L}\left( t\right) \bold{w}\left( t\right) \Delta t
O(δx(t))Δt+L(t)w(t)Δt在Update步骤使用观测方程进行估计。
从
F
(
t
)
\bold{F}(t)
F(t) 的定义中可以看到，其与 error
δ
x
(
t
)
\delta \bold{x}(t)
δx(t) 是无关的，使用上一步的估计值
x
^
(
t
)
\hat{\bold{x}}(t)
x^(t) 与这一步的控制值
u
(
t
)
\bold{u}(t)
u(t) 即可计算得到。于是，在 Predict 步骤无需设计 error 部分
δ
x
(
t
)
\delta \bold{x}(t)
δx(t) ，对
Φ
(
t
)
\Phi(t)
Φ(t) 进行近似。
离散时间模型
真实状态的运动模型方程
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
)
(1)
\bold{x}_{k}=f\left( \bold{x}_{k-1},\bold{u}_{k},\bold{w}_{k}\right) \tag{1}
xk=f(xk−1,uk,wk)(1)
由名义状态的定义，不考虑噪声项，得到名义状态的运动模型方程
x
ˉ
k
=
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
(2)
\bar{\bold{x}}_{k}=f\left( \bar{\bold{x}}_{k-1},\bold{u}_{k},\bold{0}\right) \tag{2}
xˉk=f(xˉk−1,uk,0)(2)
由定义：
x
k
=
x
ˉ
k
+
δ
x
k
\bold{x}_{k}=\bar{\bold{x}}_{k}+\delta \bold{x}_{k}
xk=xˉk+δxk，得
δ
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
)
−
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
=
f
(
x
ˉ
k
−
1
+
δ
x
k
−
1
,
u
k
,
w
k
)
−
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
=
∂
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
∂
δ
x
δ
x
k
−
1
+
1
2
!
∂
2
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
∂
δ
x
2
δ
x
k
−
1
2
+
⋯
+
∂
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
∂
w
w
k
+
1
2
!
∂
2
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
∂
w
2
w
k
2
+
⋯
=
∂
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
∂
δ
x
δ
x
k
−
1
+
O
(
δ
x
k
−
1
)
+
∂
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
∂
w
w
k
+
O
(
w
k
)
\begin{aligned} \delta \bold{x}_{k}&=f\left( \bold{x}_{k-1},\bold{u}_{k},\bold{w}_{k}\right) -f\left( \bar{\bold{x}}_{k-1},\bold{u}_{k},\bold{0}\right) \\ &=f\left( \bar{\bold{x}}_{k-1}+\delta \bold{x}_{k-1},\bold{u}_{k},\bold{w}_{k}\right) -f\left( \bar{\bold{x}}_{k-1},\bold{u}_{k},\bold{0}\right) \\ &=\dfrac{\partial f(\bar{\bold{x}}_{k-1},\bold{u}_k,\bold{0})}{\partial \delta \bold{x}}\delta \bold{x}_{k-1} +\dfrac{1}{2!}\dfrac{\partial^2 f(\bar{\bold{x}}_{k-1},\bold{u}_{k},\bold{0})}{\partial \delta \bold{x}^2}\delta \bold{x}_{k-1}^2+\cdots \\&\quad+\dfrac{\partial f(\bar{\bold{x}}_{k-1},\bold{u}_{k},\bold{0})}{\partial \bold{w}}\bold{w}_{k}+\dfrac{1}{2!}\dfrac{\partial^2 f(\bar{\bold{x}}_{k-1},\bold{u}_k,\bold{0})}{\partial \bold{w}^2}\bold{w}_k^2+\cdots \\ &=\dfrac{\partial f(\bar{\bold{x}}_{k-1},\bold{u}_k,\bold{0})}{\partial \delta \bold{x}}\delta \bold{x}_{k-1} +O\left( \delta \bold{x}_{k-1} \right) +\dfrac{\partial f(\bar{\bold{x}}_{k-1},\bold{u}_k,\bold{0})}{\partial \bold{w} }\bold{w}_k +O\left( \bold{w}_k \right) \end{aligned}
δxk=f(xk−1,uk,wk)−f(xˉk−1,uk,0)=f(xˉk−1+δxk−1,uk,wk)−f(xˉk−1,uk,0)=∂δx∂f(xˉk−1,uk,0)δxk−1+2!1∂δx2∂2f(xˉk−1,uk,0)δxk−12+⋯+∂w∂f(xˉk−1,uk,0)wk+2!1∂w2∂2f(xˉk−1,uk,0)wk2+⋯=∂δx∂f(xˉk−1,uk,0)δxk−1+O(δxk−1)+∂w∂f(xˉk−1,uk,0)wk+O(wk)
记
F
k
−
1
=
∂
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
∂
δ
x
,
L
k
=
∂
f
(
x
ˉ
k
−
1
,
u
k
,
0
)
∂
w
\bold{F}_{k-1}=\dfrac{\partial f(\bar{\bold{x}}_{k-1},\bold{u}_k,\bold{0})}{\partial \delta \bold{x}},\ \bold{L}_k=\dfrac{\partial f(\bar{\bold{x}}_{k-1},\bold{u}_k,\bold{0})}{\partial \bold{w} }
Fk−1=∂δx∂f(xˉk−1,uk,0), Lk=∂w∂f(xˉk−1,uk,0)，忽略非线性小量部分：
δ
x
k
,
w
\delta \bold{x}_k,\bold{w}
δxk,w的高阶项，则
δ
x
k
=
F
k
−
1
δ
x
k
−
1
+
L
k
w
k
(3)
\delta \bold{x}_k=\bold{F}_{k-1}\delta \bold{x}_{k-1}+\bold{L}_k\bold{w}_k\tag{3}
δxk=Fk−1δxk−1+Lkwk(3)
观测方程：
y
k
=
h
(
x
k
,
n
k
)
=
h
(
x
‾
k
+
δ
x
k
,
n
k
)
=
h
(
x
ˉ
k
+
δ
x
k
,
n
k
)
−
h
(
x
ˉ
k
,
0
)
+
h
(
x
‾
k
,
0
)
≈
h
(
x
ˉ
k
,
0
)
+
∂
h
(
x
ˉ
k
,
0
)
∂
δ
x
δ
x
k
+
∂
h
(
x
ˉ
k
,
0
)
∂
n
n
k
+
O
(
δ
x
k
)
+
O
(
n
k
)
\begin{aligned} \bold{y}_{k}&=h\left( \bold{x}_{k},\bold{n}_{k}\right) =h\left( \overline{\bold{x}}_{k}+\delta \bold{x}_{k},\bold{n}_{k}\right) \\ &=h\left( \bar{\bold{x}}_{k}+\delta \bold{x}_{k},\bold{n}_{k}\right) -h\left( \bar{\bold{x}}_{k},\bold{0}\right) +h\left( \overline{\bold{x}}_{k},\bold{0}\right) \\ &\approx h\left( \bar{\bold{x}}_{k},\bold{0}\right) + \dfrac{\partial h\left( \bar{\bold{x}}_{k},\bold{0}\right)}{\partial \delta \bold{x}}\delta \bold{x}_{k}+\dfrac{\partial h\left( \bar{\bold{x}}_{k},\bold{0}\right)}{\partial n}\bold{n}_{k}+O(\delta \bold{x}_k) +O(\bold{n}_k) \end{aligned}
yk=h(xk,nk)=h(xk+δxk,nk)=h(xˉk+δxk,nk)−h(xˉk,0)+h(xk,0)≈h(xˉk,0)+∂δx∂h(xˉk,0)δxk+∂n∂h(xˉk,0)nk+O(δxk)+O(nk)
记
H
k
−
1
=
∂
h
(
x
ˉ
k
,
0
)
∂
δ
x
,
L
k
=
∂
h
(
x
ˉ
k
,
0
)
∂
n
\bold{H}_{k-1}=\dfrac{\partial h(\bar{\bold{x}}_{k},\bold{0})}{\partial \delta \bold{x}},\ \bold{L}_k=\dfrac{\partial h(\bar{\bold{x}}_{k},\bold{0})}{\partial \bold{n} }
Hk−1=∂δx∂h(xˉk,0), Lk=∂n∂h(xˉk,0)，忽略非线性小量部分：
δ
x
k
,
n
k
\delta \bold{x}_k,\bold{n}_k
δxk,nk的高阶项，则
y
k
=
h
(
x
ˉ
k
,
0
)
+
H
k
δ
x
k
+
M
k
n
k
(4)
\bold{y}_k=h(\bar{\bold{x}}_k,\bold{0})+\bold{H}_k\delta \bold{x}_k + \bold{M}_k \bold{n}_k\tag{4}
yk=h(xˉk,0)+Hkδxk+Mknk(4)
ES-EKF过程
ES-EKF直接估计Error State,然后用它矫正Nominal State。在整个滤波过程中，我们实际上修正的变量是 δ x \delta \bold{x} δx。
- 使用运动模型更新Nominal state
x ˉ k = f ( x k − 1 , u k , 0 ) \bar{\bold{x}}_{k}=f\left( \bold{x}_{k-1},\bold{u}_{k},\bold{0}\right) xˉk=f(xk−1,uk,0)
式中的 x k − 1 \bold{x}_{k-1} xk−1是当前能获得的k-1时刻最优状态估计值。可能是前一次Prediction产生的State值(连续多次使用Motion Model)，也可能是Measurement Update后State值。 - 前向传播
我们认为前一时刻的误差状态为零均值的高斯变量
δ x k − 1 ∼ N ( 0 , P ^ k − 1 ) \delta \bold{x}_{k-1}\sim \mathcal{N}\left( \bold{0},\hat{\bold{P}}_{k-1}\right) δxk−1∼N(0,P^k−1)
由误差状态模型方程
δ x k = F k − 1 δ x k − 1 + L k w k \delta \bold{x}_{k}=\bold{F}_{k-1}\delta \bold{x}_{k-1}+\bold{L}_{k}\bold{w}_{k}\\ δxk=Fk−1δxk−1+Lkwk
概率密度向前传播得
δ x ˇ k = 0 P ˇ k = F k − 1 P ^ k − 1 F k − 1 T + L k Q k L k T \delta \check{\bold{x}}_{k}=\bold{0} \\ \check{\bold{P}}_{k}=\bold{F}_{k-1}\hat{\bold{P}}_{k-1}\bold{F}_{k-1}^{T}+\bold{L}_{k}\bold{Q}_{k}\bold{L}_{k}^{T} δxˇk=0Pˇk=Fk−1P^k−1Fk−1T+LkQkLkT - 使用测量更新后验估计
- 计算卡尔曼增益
K k = P ˇ k H k T ( H k P ˇ k H k T + M k R k M k T ) − 1 K_k = \check{\bold{P}}_k\bold{H}_k^T\left( \bold{H}_k\check{\bold{P}}_k \bold{H}_k^T+\bold{M}_k\bold{R}_k\bold{M}_k^T \right)^{-1} Kk=PˇkHkT(HkPˇkHkT+MkRkMkT)−1 - 计算误差状态后验估计
δ x ^ = δ x ˇ + K k ( y k − h ( x ˉ k , 0 ) ) P ^ k = ( I − K k H k ) P ˇ k \begin{aligned} \delta \hat{\bold{x}}&=\delta\check{\bold{x}}+\bold{K}_k\left(\bold{y}_k - h(\bar{\bold{x}}_k,\bold{0})\right) \\ \hat{\bold{P}}_k&=(\bold{I}-\bold{K}_k \bold{H}_k)\check{\bold{P}}_k \end{aligned} δx^P^k=δxˇ+Kk(yk−h(xˉk,0))=(I−KkHk)Pˇk
- 计算卡尔曼增益
- 校正名义状态
x ^ k = x ˉ k + δ x ^ k \hat{\bold{x}}_k=\bar{\bold{x}}_k+\delta\hat{\bold{x}}_k x^k=xˉk+δx^k
ESKF的优势
若状态量里面有旋转，则旋转通常被表示为欧拉角或四元数的形式。
在EKF框架中，若一个旋转状态量：
- 表示为欧拉角,三个参数三个自由度,则在优化的过程中有可能出现万向节死锁的情况。
- 表示为四元数,四个参数三个自由度,需要满足模为1的约束,优化的时候可能 会出现协方差奇异(?)的情况。
但这些问题在ESKF中被弥补。在ESKF中，一个误差旋转变量：
- 表示为欧拉角,在优化的过程中三个角度都在 0 \bold{0} 0附近,不用担心万向节死锁的情况出现。
- 表示为四元数,当一个旋转很小的时候,四元数的w=1,同样只要优化三个参数。
δ q ≃ [ 1 1 2 δ θ T ] T \delta \bold{q}\simeq\begin{bmatrix}1&\dfrac{1}{2}\delta \bm{\theta}^T\end{bmatrix}^T δq≃[121δθT]T
3995
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
