文章目录
卡尔曼滤波推导
m
o
t
i
o
n
m
o
d
e
l
:
x
k
=
A
k
−
1
x
k
−
1
+
v
k
+
w
k
,
k
=
1...
K
o
b
s
e
r
v
a
t
i
o
n
m
o
d
e
l
:
y
k
=
C
k
x
k
+
n
k
,
k
=
0...
K
\begin{aligned} motion\ model:\ {\rm x}_k &={\rm A}_{k-1} {\rm x}_{k-1}+{\rm v}_k+{\rm w}_k,\ k=1...K \\ observation\ model:\ {\rm y}_k &={\rm C}_k{\rm x}_k+{\rm n}_k,\ k=0...K \end{aligned}
motion model: xkobservation model: yk=Ak−1xk−1+vk+wk, k=1...K=Ckxk+nk, k=0...K
不考虑噪声，对状态变量和观测变量进行预测更新：
x
ˇ
k
=
A
k
x
^
k
−
1
y
ˇ
k
=
C
k
x
ˇ
k
=
C
k
A
k
x
^
k
−
1
\begin{aligned} \check{\rm x}_k &={\rm A}_k \hat{\rm x}_{k-1}\\ \check{\rm y}_k &={\rm C}_k \check{\rm x}_k={\rm C}_k {\rm A}_k\hat{\rm x}_{k-1} \end{aligned}
xˇkyˇk=Akx^k−1=Ckxˇk=CkAkx^k−1
新息：
y
~
k
=
y
k
−
y
ˇ
k
\widetilde {\rm y}_k={\rm y}_k-\check{\rm y}_k
y
k=yk−yˇk
状态变量的估计值等于状态变量的预测值加上观测变量噪声值的一部分（该部分的系数
K
k
{\rm K}_k
Kk即为卡尔曼增益矩阵）：
x
^
k
=
x
ˇ
k
+
K
k
y
~
k
=
A
k
x
^
k
−
1
+
K
k
(
y
k
−
y
ˇ
k
)
、
=
A
k
x
^
k
−
1
+
K
k
(
y
k
−
C
k
A
k
x
^
k
−
1
)
\begin{aligned} \hat{\rm x}_k &=\check{\rm x}_k+ {\rm K}_k\widetilde {\rm y}_k \\ &={\rm A}_k \hat{\rm x}_{k-1}+{\rm K}_k({\rm y}_k-\check{\rm y}_k)\\ 、 &={\rm A}_k \hat{\rm x}_{k-1}+{\rm K}_k({\rm y}_k-{\rm C}_k {\rm A}_k\hat{\rm x}_{k-1})\\ \end{aligned}
x^k、=xˇk+Kky
k=Akx^k−1+Kk(yk−yˇk)=Akx^k−1+Kk(yk−CkAkx^k−1)
状态变量的误差：
x
~
k
=
x
k
−
x
^
k
=
(
I
−
K
k
C
k
)
[
A
k
(
x
k
−
1
−
x
^
k
−
1
)
+
w
k
−
1
]
−
K
k
n
k
=
(
I
−
K
k
C
k
)
(
x
k
−
x
^
k
)
−
K
k
n
k
\begin{aligned} \widetilde {\rm x}_k &={\rm x}_k-\hat{\rm x}_k\\ &=({\rm I}-{\rm K}_k{\rm C}_k)[{\rm A}_k({\rm x}_{k-1}-\hat{\rm x}_{k-1})+{\rm w}_{k-1}]-{\rm K}_k{\rm n}_k\\ &=({\rm I}-{\rm K}_k{\rm C}_k)({\rm x}_{k}-\hat{\rm x}_{k})-{\rm K}_k{\rm n}_k \end{aligned}
x
k=xk−x^k=(I−KkCk)[Ak(xk−1−x^k−1)+wk−1]−Kknk=(I−KkCk)(xk−x^k)−Kknk
状态变量真实值与预测值之间的误差的协方差矩阵：
P
ˇ
k
=
E
[
(
x
k
−
x
ˇ
k
)
(
x
k
−
x
ˇ
k
)
T
]
\begin{aligned} \check{\rm P}_k &={\rm E}[({\rm x}_k-\check{\rm x}_k)({\rm x}_k-\check{\rm x}_k)^T]\\ \end{aligned}
Pˇk=E[(xk−xˇk)(xk−xˇk)T]
状态变量真实值与估计值之间的误差的协方差矩阵：
P
^
k
=
E
[
x
~
k
x
~
k
T
]
=
E
[
(
(
I
−
K
k
C
k
)
(
x
k
−
x
^
k
)
−
K
k
n
k
)
(
(
I
−
K
k
C
k
)
(
x
k
−
x
^
k
)
−
K
k
n
k
)
T
]
=
(
I
−
K
k
C
k
)
E
[
(
x
k
−
x
ˇ
k
)
(
x
k
−
x
ˇ
k
)
T
]
(
I
−
K
k
C
k
)
T
+
K
k
E
[
n
k
n
k
T
]
K
k
T
=
(
I
−
K
k
C
k
)
P
ˇ
k
(
I
−
K
k
C
k
)
T
+
K
k
R
k
K
k
T
=
P
ˇ
k
−
K
k
C
k
P
ˇ
k
−
P
ˇ
k
C
k
T
K
k
T
+
K
k
(
C
k
P
ˇ
k
C
k
T
+
R
k
)
K
k
T
\begin{aligned} \hat{\rm P}_k &={\rm E}[\widetilde {\rm x}_k\widetilde {\rm x}_k^T]\\ &={\rm E}[(({\rm I}-{\rm K}_k{\rm C}_k)({\rm x}_{k}-\hat{\rm x}_{k})-{\rm K}_k{\rm n}_k)(({\rm I}-{\rm K}_k{\rm C}_k)({\rm x}_{k}-\hat{\rm x}_{k})-{\rm K}_k{\rm n}_k)^T]\\ &=({\rm I}-{\rm K}_k{\rm C}_k){\rm E}[({\rm x}_k-\check{\rm x}_k)({\rm x}_k-\check{\rm x}_k)^T]({\rm I}-{\rm K}_k{\rm C}_k)^T+{\rm K}_k{\rm E}[{\rm n}_k{\rm n}_k^T]{\rm K}_k^T\\ &=({\rm I}-{\rm K}_k{\rm C}_k)\check{\rm P}_k({\rm I}-{\rm K}_k{\rm C}_k)^T+{\rm K}_k{\rm R}_k{\rm K}_k^T\\ &=\check{\rm P}_k-{\rm K}_k{\rm C}_k\check{\rm P}_k-\check{\rm P}_k{\rm C}_k^T{\rm K}_k^T+{\rm K}_k({\rm C}_k\check{\rm P}_k{\rm C}_k^T+{\rm R}_k){\rm K}_k^T \end{aligned}
P^k=E[x
kx
kT]=E[((I−KkCk)(xk−x^k)−Kknk)((I−KkCk)(xk−x^k)−Kknk)T]=(I−KkCk)E[(xk−xˇk)(xk−xˇk)T](I−KkCk)T+KkE[nknkT]KkT=(I−KkCk)Pˇk(I−KkCk)T+KkRkKkT=Pˇk−KkCkPˇk−PˇkCkTKkT+Kk(CkPˇkCkT+Rk)KkT
均方误差：
T
[
P
^
k
]
=
T
[
P
ˇ
k
]
−
2
T
[
K
k
C
k
P
ˇ
k
]
+
T
[
K
k
(
C
k
P
ˇ
k
C
k
T
+
R
k
)
K
k
T
]
\begin{aligned} T[\hat{\rm P}_k]=T[\check{\rm P}_k]-2T[{\rm K}_k{\rm C}_k\check{\rm P}_k]+T[{\rm K}_k({\rm C}_k\check{\rm P}_k{\rm C}_k^T+{\rm R}_k){\rm K}_k^T] \end{aligned}
T[P^k]=T[Pˇk]−2T[KkCkPˇk]+T[Kk(CkPˇkCkT+Rk)KkT]
最小均方误差：
d
T
[
P
^
k
]
d
K
k
=
−
2
(
C
k
P
ˇ
k
)
T
+
2
K
k
(
C
k
P
ˇ
k
C
k
T
+
R
k
)
=
0
\begin{aligned} \frac{dT[\hat{\rm P}_k]}{d{\rm K}_k}=-2({\rm C}_k\check{\rm P}_k)^T+2{\rm K}_k({\rm C}_k\check{\rm P}_k{\rm C}_k^T+{\rm R}_k)=0 \end{aligned}
dKkdT[P^k]=−2(CkPˇk)T+2Kk(CkPˇkCkT+Rk)=0
卡尔曼增益矩阵：
K
k
=
P
ˇ
k
C
k
T
(
C
k
P
ˇ
k
C
k
T
+
R
k
)
−
1
{\rm K}_k=\check{\rm P}_k{\rm C}_k^T({\rm C}_k\check{\rm P}_k{\rm C}_k^T+{\rm R}_k)^{-1}
Kk=PˇkCkT(CkPˇkCkT+Rk)−1
扫一扫
820
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
