The Iterated Kalman Filter Update as a Gauss-Newton Method
Iterated Kalman filter(IKF)是应用高斯牛顿迭代法改良Extended Kalman filter(EKF)的方法,它牺牲掉少量计算时间,通过引入非线性优化去寻找状态后验的最大似然,因而能够解决EKF线性化近似时产生的近似误差的缺点,从而在很大程度上交EKF更精确,效果更好。
符号定义与问题模型
当前系统误差: x ∈ R n x\in R^n x∈Rn
当前系统状态预测: x ^ ∈ R n \hat{x}\in R^n x^∈Rn, x ^ ∼ N ( x , P ) \hat{x}\sim N(x,P) x^∼N(x,P),其中 P P P是该状态对应协方差矩阵
系统状态观测: z ∈ R n z \in R^n z∈Rn, z ∼ N ( h ( x ) , R ) z\sim N(h(x),R) z∼N(h(x),R),其中 R R R是观测模型噪声协方差矩阵
最优系统状态估计: x ^ + \hat{x}^+ x^+, x ^ + ∼ N ( x , P + ) \hat{x}^+ \sim N(x,P^+) x^+∼N(x,P+)
观测模型: h : R n → R m h:R^n\rightarrow R^m h:Rn→Rm, h ( x ) h(x) h(x)含有随机噪声 ξ ∈ R n \xi \in R^n ξ∈Rn,其一阶导数: h ′ h' h′
问题模型:已知 z , x ^ , R , P z,\hat{x},R,P z,x^,R,P,寻找最优状态估计 x ^ + \hat{x}^+ x^+及其对应的协方差 P + P^+ P+。
IKF预测模型
IKF的预测与EKF几乎相同~
本方法应用于离散时间系统,因此设系统处于的第
i
i
i个更新周期内的最优状态估计及其协方差模为
x
^
+
=
x
i
,
P
+
=
P
i
\hat{x}^+=x_i,\ \ P^+=P_i
x^+=xi, P+=Pi
预测:
x
0
=
x
^
=
f
(
x
i
−
1
)
=
f
(
x
^
l
a
s
t
+
)
(1)
x_0=\hat{x}=f(x_{i-1})=f(\hat{x}^+_{last}) \tag1
x0=x^=f(xi−1)=f(x^last+)(1)
记
F
F
F为预测模型
f
(
x
)
f(x)
f(x)的
j
a
c
o
b
i
a
n
jacobian
jacobian,
Q
Q
Q为过程噪声的协方差矩阵,
P
0
=
P
^
+
=
F
T
P
i
−
1
F
+
Q
=
F
T
P
l
a
s
t
+
F
+
Q
(2)
P_0=\hat{P}^+=F^TP_{i-1}F+Q=F^TP^+_{last}F+Q \tag2
P0=P^+=FTPi−1F+Q=FTPlast+F+Q(2)
IKF测量更新
构建新的观测量和测量函数:
Z
=
[
z
x
^
]
,
g
(
x
)
=
[
h
(
x
)
x
]
Z=\begin{bmatrix} z\\ \hat{x} \end{bmatrix}, \ \ \ g(x)=\begin{bmatrix} h(x)\\ x \end{bmatrix}
Z=[zx^], g(x)=[h(x)x]
其中
Z
∼
N
(
g
(
x
)
,
Q
)
w
h
e
r
e
Q
=
[
R
0
0
P
]
(3)
Z\sim N(g(x),Q)\ \ \ where \ \ Q=\begin{bmatrix} R &0 \\ 0 & P \end{bmatrix} \tag3
Z∼N(g(x),Q) where Q=[R00P](3)
原本的问题模型变成:已知
Z
,
Q
,
g
Z,Q,g
Z,Q,g,寻找最优状态估计
x
^
+
\hat{x}^+
x^+及其对应的协方差
P
+
P^+
P+
定义
Z
Z
Z的似然域函数
L
(
ξ
)
L(\xi)
L(ξ),其中
Z
Z
Z里头的
x
x
x被替换成
ξ
\xi
ξ表达:
L
(
ξ
)
=
1
(
2
π
)
m
+
n
∣
Q
∣
⋅
e
x
p
(
−
1
2
(
Z
−
g
(
ξ
)
)
T
Q
−
1
(
Z
−
g
(
ξ
)
)
)
(4)
L(\xi)=\frac{1}{\sqrt{(2 \pi )^{m+n}}|Q|} \cdot exp(-\frac{1}{2}(Z-g(\xi))^TQ^{-1}(Z-g(\xi))) \tag4
L(ξ)=(2π)m+n∣Q∣1⋅exp(−21(Z−g(ξ))TQ−1(Z−g(ξ)))(4)
x
x
x的最大似然估计及其对应协方差可表达为:
x
^
+
=
a
r
g
m
a
x
(
L
(
ξ
)
)
P
+
=
(
G
T
Q
−
1
G
)
−
1
w
h
e
r
e
G
=
g
′
(
x
^
+
)
(5)
\hat{x}^+=argmax(L(\xi)) \\ P^+=(G^TQ^{-1}G)^{-1} \ \ \ where \ \ \ G=g'(\hat{x}^+) \tag5
x^+=argmax(L(ξ))P+=(GTQ−1G)−1 where G=g′(x^+)(5)
通常,对于公式
L
(
ξ
)
L(\xi)
L(ξ),我们经常取负对数将最大似然问题转换为求最小值的问题,简化后我们需要最小化的函数设为
q
(
ξ
)
q(\xi)
q(ξ):
q
(
ξ
)
=
1
2
(
Z
−
g
(
ξ
)
)
T
Q
−
1
(
Z
−
g
(
ξ
)
)
(6)
q(\xi)=\frac{1}{2}(Z-g(\xi))^TQ^{-1}(Z-g(\xi)) \tag6
q(ξ)=21(Z−g(ξ))TQ−1(Z−g(ξ))(6)
x
x
x的最大似然估计及其对应协方差可表达为:
x
^
+
=
a
r
g
m
i
n
(
q
(
ξ
)
)
(7)
\hat{x}^+=argmin(q(\xi)) \tag7
x^+=argmin(q(ξ))(7)
将公式(7)求导并等于0就可以得到最优的状态量所满足的条件:
0
=
g
′
(
ξ
)
T
Q
−
1
(
Z
−
g
(
ξ
)
)
(8)
0=g'(\xi)^TQ^{-1}(Z-g(\xi)) \tag8
0=g′(ξ)TQ−1(Z−g(ξ))(8)
根据上面的推导,公式(6)就是整个问题的优化目标函数,下面的内容主要是通过使用Gauss-Newton方法对目标函数进行优化,推导得到IKF(IEKF)的结论。
对于目标函数(6),构建非线性最小二乘问题:
m
i
n
i
m
i
z
e
f
(
ξ
)
=
1
2
∣
∣
r
(
ξ
)
∣
∣
2
(9)
minimize\ \ f(\xi)=\frac{1}{2}||r(\xi)||^2 \tag9
minimize f(ξ)=21∣∣r(ξ)∣∣2(9)
为了求解最小二乘问题,定义如下符号:
S
,
S
∗
S
=
Q
−
1
r
(
x
)
=
S
(
Z
−
g
(
x
)
)
,
r
:
R
n
→
R
n
+
m
(10)
S,\ \ S*S=Q^{-1} \\ r(x)=S(Z-g(x)), \ \ \ r:R^n\rightarrow R^{n+m}\\ \tag{10}
S, S∗S=Q−1r(x)=S(Z−g(x)), r:Rn→Rn+m(10)
r
(
x
)
r(x)
r(x)的
j
a
c
o
b
i
a
n
jacobian
jacobian可表示为:
r
′
(
x
)
=
−
S
g
′
(
x
)
(11)
r'(x)=-Sg'(x) \tag{11}
r′(x)=−Sg′(x)(11)
那么目标函数(6)可重写为:
q
(
x
)
=
1
2
∣
∣
r
(
x
)
∣
∣
2
(12)
q(x)=\frac{1}{2}||r(x)||^2 \tag{12}
q(x)=21∣∣r(x)∣∣2(12)
G-N迭代法求解:
x
i
+
1
=
x
i
+
(
g
′
(
x
i
)
T
S
T
S
g
′
(
x
i
)
)
−
1
g
′
(
x
i
)
T
S
T
S
(
Z
−
g
(
x
i
)
)
=
(
G
i
T
Q
−
1
G
i
)
−
1
G
i
T
Q
−
1
(
Z
−
g
(
x
i
)
+
G
i
x
i
)
(13)
x_{i+1}=x_i+(g'(x_i)^TS^TSg'(x_i))^{-1}g'(x_i)^TS^TS(Z-g(x_i))\\ =(G_i^TQ^{-1}G_i)^{-1}G_i^TQ^{-1}(Z-g(x_i)+G_ix_i) \tag{13}
xi+1=xi+(g′(xi)TSTSg′(xi))−1g′(xi)TSTS(Z−g(xi))=(GiTQ−1Gi)−1GiTQ−1(Z−g(xi)+Gixi)(13)
其中
G
i
=
g
′
(
x
i
)
G_i=g'(x_i)
Gi=g′(xi),
经过一系列骚操作公式,可以得到卡尔曼增益
K
i
,
K
i
=
K
(
x
i
)
K_i,\ K_i=K(x_i)
Ki, Ki=K(xi):
K
i
=
(
H
i
T
R
−
1
H
i
+
P
−
1
)
−
1
H
i
T
R
−
1
(14)
K_i=(H_i^TR^{-1}H_i+P^{-1})^{-1}H_i^TR^{-1} \tag{14}
Ki=(HiTR−1Hi+P−1)−1HiTR−1(14)
接着也是经过骚操作公式推导,公式(13)的
x
i
+
1
x_{i+1}
xi+1也可以改写:
x
i
+
1
=
x
^
+
K
i
(
z
−
h
(
x
i
)
−
H
i
(
x
^
−
x
i
)
)
(15)
x_{i+1}=\hat{x} + K_i(z-h(x_i)-H_i(\hat{x}-x_i)) \tag{15}
xi+1=x^+Ki(z−h(xi)−Hi(x^−xi))(15)
当
Δ
x
=
x
i
+
1
−
x
i
\Delta x=x_{i+1}-x_i
Δx=xi+1−xi足够小,也就是迭代收敛时,即得到最大似然
x
^
+
=
x
i
+
1
\hat{x}^+=x_{i+1}
x^+=xi+1,同样其对于的卡尔曼增益最优解为
K
+
=
K
i
+
1
K^+=K_{i+1}
K+=Ki+1
得到卡尔曼增益
K
K
K的最优解后,即可推导出对于的协方差矩阵:
P
+
=
(
I
−
K
H
)
P
P^+=(I-KH)P
P+=(I−KH)P
值得一提的是,
P
P
P在参与G-N优化时并没有进行更新,始终是上一周期IKF的输出状态协方差,
P
+
P^+
P+是在完成G-N优化后才通过最优的
K
+
K^+
K+推算得出,并进行更新。
如此,系统的测量更新形成了迭代:
参考资料:
Bell B M , Cathey F W . The iterated Kalman filter update as a Gauss-Newton method[J]. IEEE Transactions on Automatic Control, 1993, 38(2):294-297.