文章介绍了如何将卡尔曼滤波扩展到非线性系统中，通过线性化手段处理非线性模型。首先，文章提出了非线性系统的动态模型，然后讨论了初始条件和预测方程的计算，特别是如何利用雅可比矩阵进行线性化处理。接着，文章详细阐述了测量修正步骤，引入了H矩阵并计算了增益矩阵K。最后，总结了扩展卡尔曼滤波的核心思想和与线性系统中卡尔曼滤波的区别。
前言
由于在AHRS中，并不能直接使用线性方法，因此不能直接使用卡尔曼滤波，我们必须想办法把卡尔曼滤波扩展一下，以便我们在非线性系统中也能使用这种方法。由于前一章已经推导过卡尔曼滤波方程，因此我们按照上一章第四节中的公式总结部分中的各项公式来进行扩展。
一、非线性系统的模型
和线性系统不同，非线性系统的模型如下：
x
k
+
1
=
f
(
x
k
,
k
)
+
w
k
z
k
=
h
(
x
k
,
k
)
+
v
k
\begin{aligned} \bm x_{k+1}=&\bm f(\bm x_k,k)+w_k\\ \bm z_k=&\bm h(\bm x_k,k)+\bm v_k \end{aligned}
xk+1=zk=f(xk,k)+wkh(xk,k)+vk
其中，各种噪声的期望都为
0
\bm0
0，并且两个时刻之间的噪声不相关，因此描述噪声相关性的协方差矩阵只有：
Q
k
=
E
[
w
k
w
k
T
]
R
k
=
E
[
v
k
v
k
T
]
\begin{aligned} \bm Q_k=&E[\bm w_k\bm w_k^T]\\ \bm R_k=&E[\bm v_k\bm v_k^T] \end{aligned}
Qk=Rk=E[wkwkT]E[vkvkT]
二、初始条件
x ^ 0 − = E [ x 0 ] P k − = E [ ( x ^ 0 − − x 0 ) ( x ^ 0 − − x 0 ) T ] \begin{aligned} \bm {\hat x}^-_0=&E[\bm x_0]\\ \bm P^-_k=&E[(\bm {\hat x}^-_0-\bm x_0)(\bm {\hat x}^-_0-\bm x_0)^T] \end{aligned} x^0−=Pk−=E[x0]E[(x^0−−x0)(x^0−−x0)T]
三、预测方程
x
^
k
−
=
f
(
x
^
k
,
k
)
\begin{aligned} \bm {\hat x}^-_k=&\bm f(\bm {\hat x}_k,k)\\ \end{aligned}
x^k−=f(x^k,k)
对比前一章，我们发现还差一个
P
k
−
\bm P^-_k
Pk−，怎么办？？？像线性系统那样直接干是做不出来的，因此我们要使用一些线性化的手段，当然线性化也需要合理的假设，不能乱来。
我们可以看到：
P
k
−
=
E
[
(
x
^
k
−
−
x
k
)
(
x
^
k
−
−
x
k
)
T
]
=
E
{
[
f
(
x
^
k
−
1
,
k
−
1
)
−
f
(
x
k
−
1
,
k
−
1
)
−
w
k
−
1
]
[
f
(
x
^
k
−
1
,
k
−
1
)
−
f
(
x
k
−
1
,
k
−
1
)
−
w
k
−
1
]
T
}
\begin{aligned} \bm P^-_k=&E[(\bm {\hat x}^-_k-\bm x_k)(\bm {\hat x}^-_k-\bm x_k)^T]\\ =&E\{[\bm f(\bm {\hat x}_{k-1},{k-1})-\bm f(\bm x_{k-1},k-1)-\bm w_{k-1}][\bm f(\bm {\hat x}_{k-1},{k-1})-\bm f(\bm x_{k-1},k-1)-\bm w_{k-1}]^T\} \end{aligned}
Pk−==E[(x^k−−xk)(x^k−−xk)T]E{[f(x^k−1,k−1)−f(xk−1,k−1)−wk−1][f(x^k−1,k−1)−f(xk−1,k−1)−wk−1]T}
我们假设
f
(
x
^
k
,
k
)
\bm f(\bm{\hat x}_k,k)
f(x^k,k)和
f
(
x
k
,
k
)
\bm f(\bm{x}_k,k)
f(xk,k)非常接近，则如果记：
G
k
=
∂
f
∂
x
∣
x
k
=
x
^
k
\bm G_k=\frac{\partial \bm f}{\partial\bm x}|_{\bm x_k=\bm{\hat x}_k}
Gk=∂x∂f∣xk=x^k
则：
f
(
x
k
−
1
,
k
−
1
)
−
f
(
x
^
k
−
1
,
k
−
1
)
≈
G
k
(
x
k
−
x
^
k
)
\bm f(\bm x_{k-1},k-1)-\bm f(\bm {\hat x}_{k-1},{k-1}) \approx \bm G_k(\bm x_k-\bm {\hat x}_k)
f(xk−1,k−1)−f(x^k−1,k−1)≈Gk(xk−x^k)
所以我们得到：
P
k
−
≈
E
{
[
G
k
−
1
(
x
^
k
−
1
−
x
k
−
1
)
−
w
k
−
1
]
[
G
k
−
1
(
x
^
k
−
1
−
x
k
)
−
w
k
−
1
]
T
}
=
G
k
−
1
P
k
−
1
G
k
−
1
T
+
Q
k
−
1
\begin{aligned} \bm P^-_k\approx&E\{[\bm G_{k-1}(\bm {\hat x}_{k-1}-\bm x_{k-1})-\bm w_{k-1}][\bm G_{k-1}(\bm {\hat x}_{k-1}-\bm x_k)-\bm w_{k-1}]^T\}\\ =&\bm G_{k-1}\bm P_{k-1}\bm G_{k-1}^T+\bm Q_{k-1} \end{aligned}
Pk−≈=E{[Gk−1(x^k−1−xk−1)−wk−1][Gk−1(x^k−1−xk)−wk−1]T}Gk−1Pk−1Gk−1T+Qk−1
于是我们干脆就使用这个式子做协方差的预测了(其中我们假设了状态估计的偏差和噪声不相关)：
P
k
−
=
G
k
−
1
P
k
−
1
G
k
−
1
T
+
Q
k
−
1
\bm P^-_k=\bm G_{k-1}\bm P_{k-1}\bm G_{k-1}^T+\bm Q_{k-1}
Pk−=Gk−1Pk−1Gk−1T+Qk−1
四、测量修正
首先和上一章的修正思路一样，我们利用了测量信息对状态做修正：
x
^
k
=
x
^
k
−
+
K
k
[
z
k
−
h
(
x
^
k
−
,
k
)
]
\bm {\hat x}_k=\bm {\hat x}^-_k+\bm K_k[\bm z_k-\bm h(\bm {\hat x}^-_k,k)]
x^k=x^k−+Kk[zk−h(x^k−,k)]
同样我们会面临预测方程中同样的问题：
K
k
\bm K_k
Kk如何得到？？？
仿照前面思路，下面将会使用到线性化的手段，我们要研究一下，首先我们看：
x
^
k
−
x
k
=
x
^
k
−
+
K
k
z
k
−
K
k
h
(
x
^
k
−
,
k
)
−
x
k
=
x
^
k
−
+
K
k
h
(
x
k
,
k
)
+
K
k
v
k
−
K
k
h
(
x
^
k
−
,
k
)
−
x
k
=
x
^
k
−
−
x
k
−
K
k
[
h
(
x
^
k
−
,
k
)
−
h
(
x
k
,
k
)
]
+
K
k
v
k
\begin{aligned} \bm{\hat x}_k-\bm x_k=&\bm {\hat x}^-_k+\bm K_k\bm z_k-\bm K_k\bm h(\bm {\hat x}^-_k,k)-\bm x_k\\ =&\bm {\hat x}^-_k+\bm K_k\bm h(\bm x_k,k)+\bm K_k\bm v_k-\bm K_k\bm h(\bm {\hat x}^-_k,k)-\bm x_k\\ =&\bm {\hat x}^-_k-\bm x_k-\bm K_k[\bm h(\bm {\hat x}^-_k,k)-\bm h(\bm x_k,k)]+\bm K_k\bm v_k \end{aligned}
x^k−xk===x^k−+Kkzk−Kkh(x^k−,k)−xkx^k−+Kkh(xk,k)+Kkvk−Kkh(x^k−,k)−xkx^k−−xk−Kk[h(x^k−,k)−h(xk,k)]+Kkvk
和前面的手段一样，如果我们令：
H
k
=
∂
h
∂
x
∣
x
k
=
x
^
k
\bm H_k=\frac{\partial \bm h}{\partial \bm x}|_{\bm x_k=\bm {\hat x}_k}
Hk=∂x∂h∣xk=x^k
则我们有：
h
(
x
^
k
−
,
k
)
−
h
(
x
k
,
k
)
≈
H
k
(
x
k
−
−
x
k
)
\bm h(\bm {\hat x}^-_k,k)-\bm h(\bm x_k,k)\approx\bm H_k(\bm x^-_k-\bm x_k)
h(x^k−,k)−h(xk,k)≈Hk(xk−−xk)
所以：
x
^
k
−
x
k
≈
x
^
k
−
−
x
k
−
K
k
H
k
(
x
k
−
−
x
k
)
+
K
k
v
k
=
(
I
−
K
k
H
k
)
(
x
k
−
−
x
k
)
+
K
k
v
k
\begin{aligned} \bm{\hat x}_k-\bm x_k\approx&\bm {\hat x}^-_k-\bm x_k-\bm K_k\bm H_k(\bm x^-_k-\bm x_k)+\bm K_k\bm v_k\\ =&(\bm I-\bm K_k\bm H_k)(\bm x^-_k-\bm x_k)+\bm K_k\bm v_k \end{aligned}
x^k−xk≈=x^k−−xk−KkHk(xk−−xk)+Kkvk(I−KkHk)(xk−−xk)+Kkvk
所以对于目标函数，我们有：
J
=
E
{
[
(
I
−
K
k
H
k
)
(
x
k
−
−
x
k
)
+
K
k
v
k
]
T
[
(
I
−
K
k
H
k
)
(
x
k
−
−
x
k
)
+
K
k
v
k
]
}
\begin{aligned} J&=E\{[(\bm I-\bm K_k\bm H_k)(\bm x^-_k-\bm x_k)+\bm K_k\bm v_k]^T[(\bm I-\bm K_k\bm H_k)(\bm x^-_k-\bm x_k)+\bm K_k\bm v_k]\} \end{aligned}
J=E{[(I−KkHk)(xk−−xk)+Kkvk]T[(I−KkHk)(xk−−xk)+Kkvk]}
这个形式与上一章的目标函数一模一样，因此我们直接认为
K
k
\bm K_k
Kk的公式和上一章一样了。
K
k
=
P
k
−
H
k
T
[
H
k
P
k
−
H
k
T
+
R
k
]
−
1
\begin{aligned} \bm K_k=\bm P^-_k\bm H_k^T[\bm H_k\bm P^-_k\bm H_k^T+\bm R_k]^{-1} \end{aligned}
Kk=Pk−HkT[HkPk−HkT+Rk]−1
同样求取
P
−
→
P
\bm P^-\rightarrow\bm P
P−→P的过程也和上一章相同，我们有：
P
k
=
(
I
−
K
k
H
k
)
P
k
−
\begin{aligned} \bm P_k=&(\bm I-\bm K_k\bm H_k)\bm P^-_k \end{aligned}
Pk=(I−KkHk)Pk−
总结
最后我们发现扩展卡尔曼滤波区别在于原公式中的几个地方要替换，他们的思路都是一样的，差别其实比较小：
1.原来的
Φ
\bm \Phi
Φ要替换成
f
\bm f
f的导数
G
\bm G
G
2.原来的
H
\bm H
H要替换成
h
\bm h
h的导数
3.还有一个原则就是能用
f
\bm f
f和
h
\bm h
h算的地方就不要用
G
\bm G
G和
H
\bm H
H做线性近似了
扫一扫
7771
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
