对于非线性系统，先进行泰勒级数展开，略去高阶项近似为线性系统，再做线性kalman估计。
向量的泰勒级数展开
某一个含n个自变量，m维向量函数
f
(
x
)
f(x)
f(x)如下：
Y
=
f
(
x
)
,
X
=
[
x
1
x
2
.
.
.
x
n
]
T
,
Y
=
[
y
1
y
2
.
.
.
y
n
]
T
Y=f(x),X=\left[\begin{matrix}x_1&x_2&...&x_n \end{matrix}\right]^T,Y=\left[\begin{matrix}y_1&y_2&...&y_n \end{matrix}\right]^T
Y=f(x),X=[x1x2...xn]T,Y=[y1y2...yn]T
如果函数
Y
=
f
(
x
)
Y=f(x)
Y=f(x)在某个邻域内存在连续偏导数，则其泰勒展开为：
Y
=
f
(
X
0
)
+
∑
i
=
1
∞
1
i
!
(
▽
T
∗
δ
X
)
i
f
(
X
0
)
δ
X
=
X
−
X
0
▽
T
=
[
∂
∂
x
1
∂
∂
x
2
.
.
.
∂
∂
x
n
]
T
Y=f(X_0)+\sum_{i=1}^{\infty} \frac{1}{i!}(\triangledown^T *\delta X)^if(X_0) \\ \delta X=X-X_0 \\ \triangledown^T=\left[\begin{matrix} \frac{\partial}{\partial x_1}& \frac{\partial}{\partial x_2}&...& \frac{\partial}{\partial x_n}\end{matrix}\right]^T
Y=f(X0)+i=1∑∞i!1(▽T∗δX)if(X0)δX=X−X0▽T=[∂x1∂∂x2∂...∂xn∂]T
向量的一阶微分项展开（又称雅克比矩阵）
J
(
f
(
X
)
)
=
∂
f
(
X
)
∂
X
T
=
[
∂
f
1
(
x
)
∂
x
1
∂
f
1
(
x
)
∂
x
n
.
.
.
∂
f
n
(
x
)
∂
x
1
∂
f
1
(
x
)
∂
x
2
∂
f
2
(
x
)
∂
x
2
.
.
.
∂
f
2
(
x
)
∂
x
n
.
.
.
.
.
.
.
.
.
.
.
.
∂
f
m
(
x
)
∂
x
1
∂
f
m
(
x
)
∂
x
2
.
.
.
∂
f
m
(
x
)
∂
x
n
]
J(f(X))=\frac{\partial f(X)}{\partial X^T}= \left[\begin{matrix} \frac{\partial f_1(x)}{\partial x_1}&\frac{\partial f_1(x)}{\partial x_n} &...&\frac{\partial f_n(x)}{\partial x_1}\\ \frac{\partial f_1(x)}{\partial x_2}&\frac{\partial f_2(x)}{\partial x_2} &...&\frac{\partial f_2(x)}{\partial x_n}\\ ...&...&...&...\\ \frac{\partial f_m(x)}{\partial x_1}&\frac{\partial f_m(x)}{\partial x_2}&...&\frac{\partial f_m(x)}{\partial x_n}\\ \end{matrix}\right]
J(f(X))=∂XT∂f(X)=⎣⎢⎢⎢⎡∂x1∂f1(x)∂x2∂f1(x)...∂x1∂fm(x)∂xn∂f1(x)∂x2∂f2(x)...∂x2∂fm(x)............∂x1∂fn(x)∂xn∂f2(x)...∂xn∂fm(x)⎦⎥⎥⎥⎤
向量的二阶微分项展开（又称Hessian）
1
2
(
▽
T
∗
δ
X
)
2
f
(
X
0
)
=
1
2
(
∂
∂
x
1
δ
x
1
+
∂
∂
x
2
δ
x
2
+
.
.
.
+
∂
∂
x
n
δ
x
n
)
2
f
(
X
)
∣
X
=
X
0
H
(
f
j
(
X
)
)
=
▽
▽
T
f
j
(
X
)
=
[
∂
2
f
j
(
X
)
∂
x
1
2
∂
2
f
j
(
X
)
∂
x
1
∂
x
2
.
.
.
∂
2
f
j
(
X
)
∂
x
1
∂
x
n
∂
2
f
j
(
X
)
∂
x
2
∂
x
1
∂
2
f
j
(
X
)
∂
x
1
∂
x
2
.
.
.
∂
2
f
j
(
X
)
∂
x
2
∂
x
n
.
.
.
.
.
.
.
.
.
.
.
.
∂
2
f
j
(
X
)
∂
x
n
∂
x
1
∂
2
f
j
(
X
)
∂
x
n
∂
x
2
.
.
.
∂
2
f
j
(
X
)
∂
x
n
2
]
\frac{1}{2}(\triangledown^T*\delta X)^2f(X_0)=\frac{1}{2}(\frac{\partial}{\partial x_1}\delta x_1+\frac{\partial}{\partial x_2}\delta x_2+...+\frac{\partial}{\partial x_n}\delta x_n)^2f(X)|_{X=X_0} \\ H(f_j(X))=\triangledown \triangledown^Tf_j(X)\\ =\left[\begin{matrix} \frac{\partial^2 f_j(X)}{\partial x_1^2}&\frac{\partial^2 f_j(X)}{\partial x_1\partial x_2}&...&\frac{\partial^2 f_j(X)}{\partial x_1\partial x_n}\\ \frac{\partial^2 f_j(X)}{\partial x_2\partial x_1}&\frac{\partial^2 f_j(X)}{\partial x_1\partial x_2}&...&\frac{\partial^2 f_j(X)}{\partial x_2\partial x_n}\\ ...&...&...&...\\ \frac{\partial^2 f_j(X)}{\partial x_n\partial x_1}&\frac{\partial^2 f_j(X)}{\partial x_n\partial x_2}&...&\frac{\partial^2 f_j(X)}{\partial x^2_n}\\ \end{matrix}\right]
21(▽T∗δX)2f(X0)=21(∂x1∂δx1+∂x2∂δx2+...+∂xn∂δxn)2f(X)∣X=X0H(fj(X))=▽▽Tfj(X)=⎣⎢⎢⎢⎢⎡∂x12∂2fj(X)∂x2∂x1∂2fj(X)...∂xn∂x1∂2fj(X)∂x1∂x2∂2fj(X)∂x1∂x2∂2fj(X)...∂xn∂x2∂2fj(X)............∂x1∂xn∂2fj(X)∂x2∂xn∂2fj(X)...∂xn2∂2fj(X)⎦⎥⎥⎥⎥⎤
一阶EKF
离散系统状态空间非线性模型
X
k
:
n
维
状
态
向
量
X_k:n维状态向量
Xk:n维状态向量
Z
k
:
m
维
测
量
向
量
Z_k:m维测量向量
Zk:m维测量向量
f
(
X
k
)
:
n
维
非
线
性
函
数
f(X_{k}):n维非线性函数
f(Xk):n维非线性函数
Γ
k
/
k
−
1
:
已
知
的
系
统
结
构
参
数
，
分
别
为
n
×
l
阶
系
统
分
配
噪
声
\Gamma_{k/k-1}:已知的系统结构参数，分别为n×l阶系统分配噪声
Γk/k−1:已知的系统结构参数，分别为n×l阶系统分配噪声
H
(
X
k
)
:
m
维
非
线
性
函
数
H(X_k):m维非线性函数
H(Xk):m维非线性函数
V
k
:
m
维
测
量
噪
声
，
高
斯
白
噪
声
，
服
从
正
太
分
布
V_k:m维测量噪声，高斯白噪声，服从正太分布
Vk:m维测量噪声，高斯白噪声，服从正太分布
W
k
−
1
:
m
维
系
统
噪
声
向
量
，
高
斯
白
噪
声
，
服
从
正
太
分
布
W_{k-1}:m维系统噪声向量，高斯白噪声，服从正太分布
Wk−1:m维系统噪声向量，高斯白噪声，服从正太分布
V
k
与
W
k
−
1
互
不
相
关
V_k与W_{k-1}互不相关
Vk与Wk−1互不相关
{
X
k
=
f
(
X
k
−
1
)
+
Γ
k
/
k
−
1
W
k
−
1
Z
k
=
h
(
X
k
)
+
V
k
s
t
.
{
E
[
W
k
]
=
0
,
E
[
W
k
W
j
T
]
=
Q
k
δ
k
j
Q
k
≥
0
E
[
V
k
]
=
0
,
E
[
V
k
V
j
T
]
=
R
k
δ
k
j
,
E
[
W
k
V
j
T
]
=
0
R
≥
0
\begin{cases} X_k=f(X_{k-1})+\Gamma_{k/k-1}W_{k-1}\\ Z_k=h(X_k)+V_k\\ \end{cases} \\ st. \\ \begin{cases} E[W_k]=0,E[W_kW_j^T]=Q_k\delta_{kj} &Q_k \geq 0\\ E[V_k]=0,E[V_kV_j^T]=R_k\delta_{kj},E[W_kV_j^T]=0&R\geq 0\\ \end{cases}
{Xk=f(Xk−1)+Γk/k−1Wk−1Zk=h(Xk)+Vkst.{E[Wk]=0,E[WkWjT]=QkδkjE[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0Qk≥0R≥0
已知
k
−
1
k-1
k−1时刻
X
k
−
1
X_{k-1}
Xk−1的一个参考值(理想值)为
X
k
−
1
n
X^n_{k-1}
Xk−1n
-
则参考值与真实值之间偏差为:
δ X k − 1 = X k − 1 − X k − 1 n \delta X_{k-1}=X_{k-1}-X_{k-1}^n δXk−1=Xk−1−Xk−1n -
当系统噪声为零均值时,得到预测为：
X k / k − 1 n = f ( X k − 1 n ) X_{k/k-1}^n=f(X_{k-1}^n) Xk/k−1n=f(Xk−1n)
状态预测的偏差为： δ X k − 1 = X k − 1 − X k / k − 1 n \delta X_{k-1}=X_{k-1}-X_{k/k-1}^n δXk−1=Xk−1−Xk/k−1n -
利用 X k / k − 1 n X_{k/k-1}^n Xk/k−1n得到预测值:
Z k / k − 1 n = h ( X k / k − 1 n ) Z^n_{k/k-1}=h(X^n_{k/k-1}) Zk/k−1n=h(Xk/k−1n)
量测偏差为： δ Z k = Z k − Z k / k − 1 n \delta Z_k=Z_k-Z^n_{k/k-1} δZk=Zk−Zk/k−1n -
将 f ( ) f() f()在k-1时刻的参考值 X k − 1 n X^n_{k-1} Xk−1n邻域内泰勒展开，并取一阶近似可得 X k X_k Xk
Φ k / k − 1 n = J ( f ( X k − 1 n ) ) \Phi^n_{k/k-1}=J(f(X^n_{k-1})) Φk/k−1n=J(f(Xk−1n))
X k ≈ f ( X k − 1 n ) + J ( f ( X k − 1 n ) ) ( X k − 1 − X k − 1 n ) + Γ k − 1 W k − 1 = X k / k − 1 n + Φ k / k − 1 n ( X k − 1 − X k − 1 n ) + Γ k − 1 W k − 1 X_k \approx f(X_{k-1}^n)+J(f(X^n_{k-1}))(X_{k-1}-X_{k-1}^n)+\Gamma_{k-1}W_{k-1} \\ =X^n_{k/k-1}+\Phi^n_{k/k-1}(X_{k-1}-X_{k-1}^n)+\Gamma_{k-1}W_{k-1} Xk≈f(Xk−1n)+J(f(Xk−1n))(Xk−1−Xk−1n)+Γk−1Wk−1=Xk/k−1n+Φk/k−1n(Xk−1−Xk−1n)+Γk−1Wk−1
得到: X k − X k / k − 1 n = Φ k / k − 1 n ( X k − 1 − X k − 1 n ) + Γ k − 1 W k − 1 X_k-X^n_{k/k-1}=\Phi^n_{k/k-1}(X_{k-1}-X^n_{k-1})+\Gamma_{k-1}W_{k-1} Xk−Xk/k−1n=Φk/k−1n(Xk−1−Xk−1n)+Γk−1Wk−1 -
将 h ( ) h() h()在k-1时刻的参考值 X k − 1 n X^n_{k-1} Xk−1n邻域内泰勒展开，并取一阶近似可得 X k X_k Xk
H k n = J ( h ( X k − X k / k − 1 n ) ) H_k^n=J(h(X_k-X^n_{k/k-1})) Hkn=J(h(Xk−Xk/k−1n))
Z k ≈ h ( X k / k − 1 n ) + J ( h ( X k / k − 1 n ) ) ( X k − X k / k − 1 n ) + V k = Z k / k − 1 n + H k n ( X k − X k / k − 1 n ) + V k Z_k\approx h(X^n_{k/k-1})+J(h(X^n_{k/k-1}))(X_k-X^n_{k/k-1})+V_k \\ =Z^n_{k/k-1}+H^n_k(X_k-X^n_{k/k-1})+V_k Zk≈h(Xk/k−1n)+J(h(Xk/k−1n))(Xk−Xk/k−1n)+Vk=Zk/k−1n+Hkn(Xk−Xk/k−1n)+Vk
得到: Z k − Z k / k − 1 n = H k n ( X k − X k / k − 1 n ) + V k Z_k-Z^n_{k/k-1}=H^n_k(X_k-X^n_{k/k-1})+V_k Zk−Zk/k−1n=Hkn(Xk−Xk/k−1n)+Vk -
使用偏差构成一个新的系统
{ δ X k = Φ k / k − 1 n δ X k − 1 + Γ k − 1 W k − 1 δ Z k = H k n δ X k + V k \begin{cases} \delta X_k=\Phi^n_{k/k-1}\delta X_{k-1}+\Gamma_{k-1}W_{k-1}\\ \delta Z_k=H_k^n \delta X_k+V_k\\ \end{cases} {δXk=Φk/k−1nδXk−1+Γk−1Wk−1δZk=HknδXk+Vk -
从而得到偏差状态线性系统的kalman滤波方程
{ δ X ^ k / k − 1 = Φ k / k − 1 n δ X ^ k − 1 P k / k − 1 = Φ k / k − 1 n P k − 1 ( Φ k / k − 1 n ) T + Γ k − 1 Q k − 1 Γ k − 1 T K k n = P k / k − 1 ( H k n ) T [ H k P k / k − 1 ( H k n ) T + R k ] − 1 δ X ^ k = δ X ^ k / k − 1 + K k n ( δ Z k − H k n δ X ^ k / k − 1 ) P k = ( I − K k n H k n ) P k / k − 1 \begin{cases} \delta \hat X_{k/k-1}=\Phi_{k/k-1}^n\delta \hat X_{k-1}\\ P_{k/k-1}=\Phi^n_{k/k-1}P_{k-1}(\Phi^n_{k/k-1})^T+\Gamma_{k-1}Q_{k-1}\Gamma^T_{k-1}\\ K^n_k=P_{k/k-1}(H_k^n)^T[H_kP_{k/k-1}(H^n_k)^T+R_k]^{-1}\\ \delta \hat X_k=\delta \hat X_{k/k-1}+K^n_k(\delta Z_k-H^n_k\delta \hat X_{k/k-1})\\ P_k=(I-K_k^nH_k^n)P_{k/k-1}\\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧δX^k/k−1=Φk/k−1nδX^k−1Pk/k−1=Φk/k−1nPk−1(Φk/k−1n)T+Γk−1Qk−1Γk−1TKkn=Pk/k−1(Hkn)T[HkPk/k−1(Hkn)T+Rk]−1δX^k=δX^k/k−1+Kkn(δZk−HknδX^k/k−1)Pk=(I−KknHkn)Pk/k−1 -
经偏差状态线性系统，推导出EKF
已知：
X ^ k = X k / k − 1 n + δ X ^ k = f ( X k − 1 n ) + δ X ^ k \hat X_k=X^n_{k/k-1}+\delta \hat X_k=f(X^n_{k-1})+\delta \hat X_k X^k=Xk/k−1n+δX^k=f(Xk−1n)+δX^k
δ X ^ k − 1 = X ^ k − 1 − X k − 1 n \delta \hat X_{k-1}=\hat X_{k-1}-X^n_{k-1} δX^k−1=X^k−1−Xk−1n
δ X ^ k = X ^ k − X k / k − 1 n \delta \hat X_k=\hat X_k-X^n_{k/k-1} δX^k=X^k−Xk/k−1n
{ X ^ k / k − 1 = f ( X ^ k − 1 ) P k / k − 1 = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 X ^ k = X ^ k / k − 1 + K k [ Z k − h ( X ^ k / k − 1 ) ] P k = ( I − K k H k ) P k / k − 1 Φ k / k − 1 = J ( f ( X ^ k − 1 ) , H k = J ( h ( X ^ k / k − 1 ) ) ) \begin{cases} \hat X_{k/k-1}=f(\hat X_{k-1})\\ P_{k/k-1}=\Phi_{k/k-1}P_{k-1}\Phi^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T\\ K_k=P_{k/k-1}H_k^T(H_kP_{k/k-1}H^T_k+R_k)^{-1}\\ \hat X_k=\hat X_{k/k-1}+K_k[Z_k-h(\hat X_{k/k-1})]\\ P_k=(I-K_kH_k)P_{k/k-1}\\ \Phi_{k/k-1}=J(f(\hat X_{k-1}),H_k=J(h(\hat X_{k/k-1})))\\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧X^k/k−1=f(X^k−1)Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1TKk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1X^k=X^k/k−1+Kk[Zk−h(X^k/k−1)]Pk=(I−KkHk)Pk/k−1Φk/k−1=J(f(X^k−1),Hk=J(h(X^k/k−1)))
二阶EKF
-
将状态方程在参考值 X k − 1 n = X ^ k − 1 X^n_{k-1}=\hat X_{k-1} Xk−1n=X^k−1处展开可得:
δ X = X k − 1 − X k − 1 n \delta X=X_{k-1}-X_{k-1}^n δX=Xk−1−Xk−1n
Φ k / k − 1 n = J ( f ( X k − 1 n ) ) \Phi^n_{k/k-1}=J(f(X^n_{k-1})) Φk/k−1n=J(f(Xk−1n))
D f i = H ( f i ( X k − 1 n ) ) D_{fi}=H(f_i(X^n_{k-1})) Dfi=H(fi(Xk−1n))
X k = f ( X k − 1 n ) + Φ k / k − 1 n δ X + 1 2 ∑ i = 1 n e i t r ( D f i ∗ δ X δ X T ) + Γ k − 1 W k − 1 X_k=f(X^n_{k-1})+\Phi^n_{k/k-1}\delta X+\frac{1}{2}\sum_{i=1}^ne_itr(D_{fi}*\delta X \delta X^T)+\Gamma_{k-1}W_{k-1} Xk=f(Xk−1n)+Φk/k−1nδX+21i=1∑neitr(Dfi∗δXδXT)+Γk−1Wk−1 -
假设 δ X ∼ N ( 0 , P k − 1 ) \delta X \sim N(0,P_{k-1}) δX∼N(0,Pk−1),求期望可以得到:
X ^ k / k − 1 = E [ X k ] = E [ f ( X k − 1 n ) + Φ k / k − 1 n δ X + 1 2 ∑ i = 1 n e i t r ( D f i ∗ δ X δ X T ) + Γ k − 1 W k − 1 ] = f ( X k − 1 n ) + Φ k / k − 1 n E [ δ X ] + 1 2 ∑ i = 1 n e i t r ( D f i ∗ E [ δ X δ X T ] ) + Γ k − 1 E [ W k − 1 ] = f ( X k − 1 n ) + 1 2 ∑ i = 1 n e i t r ( D f i P k − 1 ) \hat X_{k/k-1}=E[X_k]=E[f(X^n_{k-1})+\Phi^n_{k/k-1}\delta X+\frac{1}{2}\sum_{i=1}^ne_itr(D_{fi}*\delta X \delta X^T)+\Gamma_{k-1}W_{k-1}] \\ =f(X^n_{k-1})+\Phi^n_{k/k-1}E[\delta X]+\frac{1}{2}\sum_{i=1}^{n}e_itr(D_{fi}*E[\delta X \delta X^T])+\Gamma_{k-1}E[W_{k-1}] \\ =f(X^n_{k-1})+\frac{1}{2}\sum_{i=1}^{n}e_itr(D_{fi}P_{k-1}) X^k/k−1=E[Xk]=E[f(Xk−1n)+Φk/k−1nδX+21i=1∑neitr(Dfi∗δXδXT)+Γk−1Wk−1]=f(Xk−1n)+Φk/k−1nE[δX]+21i=1∑neitr(Dfi∗E[δXδXT])+Γk−1E[Wk−1]=f(Xk−1n)+21i=1∑neitr(DfiPk−1) -
状态一步误差:
X ~ k / k − 1 = X k − X ^ k / k − 1 = Φ k / k − 1 n δ X + 1 2 ∑ i = 1 n e i t r ( D j i ∗ δ X δ X T ) + Γ k − 1 W k − 1 − 1 2 ∑ i = 1 n e i t r ( D f i P k − 1 ) = Φ k / k − 1 n δ X + 1 2 ∑ i = 1 n e i t r ( D f i ( δ X δ X T − P k − 1 ) ) + Γ k − 1 W k − 1 \tilde X_{k/k-1}=X_k-\hat X_{k/k-1}\\ =\Phi^n_{k/k-1}\delta X+\frac{1}{2}\sum^n_{i=1}e_itr(D_{ji}*\delta X \delta X^T)+\Gamma_{k-1}W_{k-1}-\frac{1}{2}\sum_{i=1}^{n}e_itr(D_{fi}P_{k-1}) \\ =\Phi^n_{k/k-1}\delta X+\frac{1}{2}\sum_{i=1}^ne_i tr(D_{fi}(\delta X \delta X^T-P_{k-1}))+\Gamma_{k-1}W_{k-1} X~k/k−1=Xk−X^k/k−1=Φk/k−1nδX+21i=1∑neitr(Dji∗δXδXT)+Γk−1Wk−1−21i=1∑neitr(DfiPk−1)=Φk/k−1nδX+21i=1∑neitr(Dfi(δXδXT−Pk−1))+Γk−1Wk−1 -
状态一步预测预测 X ^ k / k − 1 \hat X_{k/k-1} X^k/k−1的均方误差阵为:
P k / k − 1 = E [ X ~ k / k − 1 X ~ k / k − 1 T ] = E [ [ Φ k / k − 1 n δ X + 1 2 ∑ i = 1 n e i t r ( D f i ( δ X δ X T − P k − 1 ) ) + Γ k − 1 W k − 1 ] × [ Φ k / k − 1 n δ X + 1 2 ∑ i = 1 n e i t r ( D f i ( δ X δ X T − P k − 1 ) ) + Γ k − 1 W k − 1 ] ] P_{k/k-1}=E[\tilde X_{k/k-1}\tilde X_{k/k-1}^T]\\ =E[[\Phi^n_{k/k-1}\delta X+\frac{1}{2}\sum^n_{i=1}e_itr(D_{fi}(\delta X\delta X^T-P_{k-1}))+\Gamma_{k-1}W_{k-1}]×\\ [\Phi^n_{k/k-1}\delta X+\frac{1}{2}\sum_{i=1}^{n}e_itr(D_{fi}(\delta X \delta X^T-P_{k-1}))+\Gamma_{k-1}W_{k-1}]] Pk/k−1=E[X~k/k−1X~k/k−1T]=E[[Φk/k−1nδX+21i=1∑neitr(Dfi(δXδXT−Pk−1))+Γk−1Wk−1]×[Φk/k−1nδX+21i=1∑neitr(Dfi(δXδXT−Pk−1))+Γk−1Wk−1]]
考虑到 δ X \delta X δX是对称分布的，其奇次阶矩为0，且 δ X \delta X δX和 W k − 1 W_{k-1} Wk−1之间不相关，进一步得到:
∑ j = 1 n e i e j t r ( D f i P k − 1 D f j P k − 1 ) : 2 阶 修 正 项 \sum_{j=1}^ne_ie_jtr(D_{fi}P_{k-1}D_{fj}P_{k-1}):2阶修正项 ∑j=1neiejtr(DfiPk−1DfjPk−1):2阶修正项
P k / k − 1 = Φ k / k − 1 n P k − 1 ( Φ k / k − 1 n ) T + 1 2 ∑ i = 1 n ∑ j = 1 n e i e j t r ( D f i P k − 1 D f j P k − 1 ) + Γ k − 1 Q k − 1 Γ k − 1 T P_{k/k-1}=\Phi^n_{k/k-1}P_{k-1}(\Phi^n_{k/k-1})^T+\frac{1}{2}\sum_{i=1}^n\sum_{j=1}^ne_ie_jtr(D_{fi}P_{k-1}D_{fj}P_{k-1})+\Gamma_{k-1}Q_{k-1}\Gamma^T_{k-1}\\ Pk/k−1=Φk/k−1nPk−1(Φk/k−1n)T+21i=1∑nj=1∑neiejtr(DfiPk−1DfjPk−1)+Γk−1Qk−1Γk−1T -
在参考值 X k / k − 1 n = X ^ k / k − 1 X^n_{k/k-1}=\hat X_{k/k-1} Xk/k−1n=X^k/k−1,附近二阶泰勒级数展开的量测方程，有：
δ X = X k − X k / k − 1 n \delta X=X_k-X^n_{k/k-1} δX=Xk−Xk/k−1n
H k n = J ( h ( X k / k − 1 n ) ) H^n_k=J(h(X^n_{k/k-1})) Hkn=J(h(Xk/k−1n))
D h i = H ( h i ( X k / k − 1 n ) ) D_{hi}=H(h_i(X^n_{k/k-1})) Dhi=H(hi(Xk/k−1n))
δ X ∼ N ( 0 , P k / k − 1 ) \delta X \sim N(0,P_{k/k-1}) δX∼N(0,Pk/k−1)
Z k / k − 1 = h ( X k / k − 1 n ) + H k n δ X + 1 2 ∑ i = 1 m e i t r ( D h i ∗ δ X δ X T ) + V k Z_{k/k-1}=h(X^n_{k/k-1})+H_k^n\delta X+\frac{1}{2}\sum_{i=1}^{m}e_itr(D_{hi}*\delta X\delta X^T)+V_k Zk/k−1=h(Xk/k−1n)+HknδX+21i=1∑meitr(Dhi∗δXδXT)+Vk
Z ^ k / k − 1 = h ( X k / k − 1 n ) + 1 2 ∑ i = 1 m e i t r ( D h i ∗ P k / k − 1 ) \hat Z_{k/k-1}=h(X^n_{k/k-1})+\frac{1}{2}\sum_{i=1}^me_itr(D_{hi}*P_{k/k-1}) Z^k/k−1=h(Xk/k−1n)+21i=1∑meitr(Dhi∗Pk/k−1)
P Z Z , k / k − 1 = H k n P k / k − 1 ( H k n ) T + 1 2 ∑ i = 1 m ∑ j = 1 m e i e j T t r ( D h i P k / k − 1 D h i P k / k − 1 ) + R k P X Z , k / k − 1 = P k / k − 1 H k n P_{ZZ,k/k-1}=H^n_kP_{k/k-1}(H^n_k)^T+\frac{1}{2}\sum_{i=1}^m\sum_{j=1}^me_ie_j^Ttr(D_{hi}P_{k/k-1}D_{hi}P_{k/k-1})+R_k \\ P_{XZ,k/k-1}=P_{k/k-1}H^n_k PZZ,k/k−1=HknPk/k−1(Hkn)T+21i=1∑mj=1∑meiejTtr(DhiPk/k−1DhiPk/k−1)+RkPXZ,k/k−1=Pk/k−1Hkn -
得到在点 X k − 1 n = X ^ k − 1 , X k / k − 1 n = X ^ k / k − 1 X^n_{k-1}=\hat X_{k-1},X^n_{k/k-1}=\hat X_{k/k-1} Xk−1n=X^k−1,Xk/k−1n=X^k/k−1
{ X ^ k / k − 1 = f ( X ^ k − 1 ) + 1 2 ∑ i = 1 n e i t r ( D f i P k − 1 ) P k / k − 1 = Φ k / k − 1 P k − 1 Φ k / k − 1 T + 1 2 ∑ i = 1 n ∑ j = 1 n e i e j T t r ( D f i P k − 1 D f j P k − 1 ) + Γ k − 1 Q k − 1 Γ k − 1 T K k = P k / k − 1 H k T [ H k P k / k − 1 H k T + 1 2 ∑ i = 1 n ∑ j = 1 n e i e j T t r ( D f i P k − 1 D f j P k − 1 ) + R k ] − 1 X ^ k = X ^ k / k − 1 + K k [ Z k − h ( X ^ k / k − 1 ) − 1 2 ∑ i = 1 m e i t r ( D h i P k / k − 1 ) ] P k = ( I − K k H k ) P k / k − 1 \begin{cases} \hat X_{k/k-1}=f(\hat X_{k-1})+\frac{1}{2}\sum^n_{i=1}e_itr(D_{fi}P_{k-1})\\ P_{k/k-1}=\Phi_{k/k-1}P_{k-1}\Phi^T_{k/k-1}+\frac{1}{2}\sum^n_{i=1}\sum^n_{j=1}e_iej^Ttr(D_{fi}P_{k-1}D_{fj}P_{k-1})+\Gamma_{k-1}Q_{k-1}\Gamma^T_{k-1}\\ K_k=P_{k/k-1}H^T_k[H_kP_{k/k-1}H^T_k+\frac{1}{2}\sum^n_{i=1}\sum^n_{j=1}e_iej^Ttr(D_{fi}P_{k-1}D_{fj}P_{k-1})+R_k]^{-1}\\ \hat X_k=\hat X_{k/k-1}+K_k[Z_k-h(\hat X_{k/k-1})-\frac{1}{2}\sum_{i=1}^me_i tr(D_{hi}P_{k/k-1})]\\ P_k=(I-K_kH_k)P_{k/k-1}\\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧X^k/k−1=f(X^k−1)+21∑i=1neitr(DfiPk−1)Pk/k−1=Φk/k−1Pk−1Φk/k−1T+21∑i=1n∑j=1neiejTtr(DfiPk−1DfjPk−1)+Γk−1Qk−1Γk−1TKk=Pk/k−1HkT[HkPk/k−1HkT+21∑i=1n∑j=1neiejTtr(DfiPk−1DfjPk−1)+Rk]−1X^k=X^k/k−1+Kk[Zk−h(X^k/k−1)−21∑i=1meitr(DhiPk/k−1)]Pk=(I−KkHk)Pk/k−1
迭代滤波
未获得测量值时:
X
^
k
−
1
\hat X_{k-1}
X^k−1是状态
X
k
−
1
X_{k-1}
Xk−1的最优估计
在获得量测值时：一步反向平滑值
X
^
k
−
1
/
k
\hat X_{k-1/k}
X^k−1/k精度高于
X
^
k
−
1
\hat X_{k-1}
X^k−1
因此可以在获得测量值之后，以平滑值
X
^
k
−
1
/
k
\hat X_{k-1/k}
X^k−1/k取代
X
^
k
−
1
\hat X_{k-1}
X^k−1
使用RTS平滑算法求取平滑值：
X
^
k
/
k
−
1
=
X
^
k
−
1
+
P
k
−
1
(
Φ
)
T
(
P
k
/
k
−
1
)
−
1
(
X
^
k
−
X
^
k
/
k
−
1
)
\hat X_{k/k-1}=\hat X_{k-1}+P_{k-1}(\Phi)^T(P_{k/k-1})^{-1}(\hat X_k-\hat X_{k/k-1})
X^k/k−1=X^k−1+Pk−1(Φ)T(Pk/k−1)−1(X^k−X^k/k−1)
迭代滤波公式为：
{
X
^
k
/
k
−
1
=
f
(
X
^
k
−
1
/
k
)
+
Φ
k
/
k
−
1
(
X
^
k
−
1
−
X
^
k
−
1
/
k
)
P
k
/
k
−
1
=
Φ
k
/
k
−
1
P
k
−
1
Φ
k
/
k
−
1
T
+
Γ
k
−
1
Q
k
−
1
Γ
k
−
1
T
K
k
=
P
k
/
k
−
1
H
k
T
(
H
k
P
k
/
k
−
1
H
k
T
+
R
k
)
−
1
X
^
k
=
X
^
k
/
k
−
1
+
K
k
[
Z
k
−
h
(
X
^
k
∗
)
−
H
k
(
X
^
k
/
k
−
1
−
X
^
k
∗
)
]
P
k
=
(
I
−
K
k
H
k
)
P
k
/
k
−
1
Φ
k
/
k
−
1
=
J
(
f
(
X
^
k
−
1
/
k
)
)
;
H
k
=
J
(
h
(
X
^
k
∗
)
)
\begin{cases} \hat X_{k/k-1}=f(\hat X_{k-1/k})+\Phi_{k/k-1}(\hat X_{k-1}-\hat X_{k-1/k})\\ P_{k/k-1}=\Phi_{k/k-1}P_{k-1}\Phi^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T\\ K_k=P_{k/k-1}H^T_k(H_kP_{k/k-1}H_k^T+R_k)^{-1}\\ \hat X_k=\hat X_{k/k-1}+K_k[Z_k-h(\hat X_k^*)-H_k(\hat X_{k/k-1}-\hat X_k^*)]\\ P_k=(I-K_kH_k)P_{k/k-1}\\ \Phi_{k/k-1}=J(f(\hat X_{k-1/k}));H_k=J(h(\hat X_k^*))\\ \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧X^k/k−1=f(X^k−1/k)+Φk/k−1(X^k−1−X^k−1/k)Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1TKk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1X^k=X^k/k−1+Kk[Zk−h(X^k∗)−Hk(X^k/k−1−X^k∗)]Pk=(I−KkHk)Pk/k−1Φk/k−1=J(f(X^k−1/k));Hk=J(h(X^k∗))
6万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
