__________________2018.1.17 全部更新完毕 ♥ 转载请与作者联系_________________
(课程作业code见GitHub) 由于水平有限,自己写的一些简单代码可能存在问题,还希望读者给出批评指正。
提到卡尔曼滤波,要有一个宏观的概念。
在估计与滤波这门课上我接触到了卡尔曼滤波,首先,整体的思想是,我是用来做估计的。
给出动态系统的两个方程(Consider the following dynamic equation):
xk+1=Fkxk+Gkuk+wk k=0,1,...
x
k
+
1
=
F
k
x
k
+
G
k
u
k
+
w
k
k
=
0
,
1
,
.
.
.
where
wk,k=0,1,...
w
k
,
k
=
0
,
1
,
.
.
.
,is a white Gaussian, possibly non-stationary, sequence of zero-mean with covariance
E[wkw′k]=Qk
E
[
w
k
w
k
′
]
=
Q
k
The measurement equation is
zk=Hkxk+vk
z
k
=
H
k
x
k
+
v
k
where
vk
v
k
is a zero-mean white Gaussian, possibly non-stationary, sequence of covariance
E[vkv′k]=Rk
E
[
v
k
v
k
′
]
=
R
k
It is assumed that
x0∼N(x¯0,P0)
x
0
∼
N
(
x
¯
0
,
P
0
)
Also, the two noise sequences and
x0
x
0
are assumed mutually independent.
The above constitutes the linear Gaussian (LG) assumption.
What is optimal state estimation problem?——Estimate
xj
x
j
given
zk≜{zi,i≤k}
z
k
≜
{
z
i
,
i
≤
k
}
under some optimality criterion.
根据以上给出的条件,我们知道了卡尔曼滤波所要做的工作——“估计(Estimate)”,根据系统状态和数据给出的时刻的不同又要分为三种具体的情况考虑。
Three types of state estimations problems:
- filtering if j=k j = k
- smoothing if j<k j < k
- prediction if j>k j > k
这里,
j
j
是状态的时刻,是测量数据的时刻。并且
j,k
j
,
k
在上标(如
zk
z
k
)是一种标记,意味着一个set(集合
zk≜{zi,i≤k}
z
k
≜
{
z
i
,
i
≤
k
}
)。 在下标意味着某一时刻的一个值。
简单来说,有当前时刻的状态和当前时刻的数据时,意味着进行滤波;有前一时刻的状态和当前时刻的数据,意味着平滑;有当前时刻的状态而数据是上一时刻的,意味着预测。
Under the above linear Gaussian formulation, the MMSE (filtering) estimate of the state is
x^k|k=E[xk|zk]
x
^
k
|
k
=
E
[
x
k
|
z
k
]
【how to get mean? 使用了条件均值,求均值意味着求积分】
with the associated MSE matrix
Pk|k=MSE(x^k|k|zk)=E[(xk−x^k|k)′|zk]=cov(xk|zk)
P
k
|
k
=
M
S
E
(
x
^
k
|
k
|
z
k
)
=
E
[
(
x
k
−
x
^
k
|
k
)
′
|
z
k
]
=
c
o
v
(
x
k
|
z
k
)
【给出了条件协方差,即真实状态的函数】
这里提到MMSE作为度量优化准则即回答 how about the result的问题。MMSE容易得到mean, 比MAP使用density 计算简单。MSE矩阵给出了how sure you are, 即回答你的估计有多可靠。
【卡尔曼滤波与之前学过的MMSE和MSE对比】
The Kalman filter provides a recursive implementation of this in that within one cycle, x^k|k x ^ k | k and Pk|k P k | k are evolved into x^k+1|k+1 x ^ k + 1 | k + 1 and Pk+1|k+1 P k + 1 | k + 1 .
Recall that if two random vectors x x and are jointly Gaussian and z z is an observation about the unknown parameter ,one has
x^MMSE=E[x|z]=x¯+CxzC−1z(z−z¯)
x
^
M
M
S
E
=
E
[
x
|
z
]
=
x
¯
+
C
x
z
C
z
−
1
(
z
−
z
¯
)
P=MSE(x^MMSE|z)=Cx−CxzC−1zCzx
P
=
M
S
E
(
x
^
M
M
S
E
|
z
)
=
C
x
−
C
x
z
C
z
−
1
C
z
x
【此处是卡尔曼滤波的推理开始,当我们得到了k时刻的状态参数和其最优估计值,当得到k+1时刻的数据之后我们就可以得到k+1时刻的状态参数估计值以及k+1时刻的数据估计。】
By treating dynamic estimation as a recursive static estimation, the Kalman filter can then be abtained with the following replacement:
x→xk+1
x
→
x
k
+
1
x¯→x^k+1|k=E[xk+1|zk]
x
¯
→
x
^
k
+
1
|
k
=
E
[
x
k
+
1
|
z
k
]
z→zk+1
z
→
z
k
+
1
z¯→z^k+1|k=E[zk+1|zk]
z
¯
→
z
^
k
+
1
|
k
=
E
[
z
k
+
1
|
z
k
]
x^→x^k+1|k+1=E[xk+1|zk+1]
x
^
→
x
^
k
+
1
|
k
+
1
=
E
[
x
k
+
1
|
z
k
+
1
]
Cx→Pk+1|k=cov(xk+1|zk)=cov(x~k+1|k|zk)=MSE(x^k+1|k|zk)
C
x
→
P
k
+
1
|
k
=
c
o
v
(
x
k
+
1
|
z
k
)
=
c
o
v
(
x
~
k
+
1
|
k
|
z
k
)
=
M
S
E
(
x
^
k
+
1
|
k
|
z
k
)
Cz→Sk+1=cov(zk+1|zk)=cov(z~k+1|k|zk)
C
z
→
S
k
+
1
=
c
o
v
(
z
k
+
1
|
z
k
)
=
c
o
v
(
z
~
k
+
1
|
k
|
z
k
)
Cxz→cov(xk+1,zk+1|zk)=cov(x~k+1|k,z~k+1|k|zk)
C
x
z
→
c
o
v
(
x
k
+
1
,
z
k
+
1
|
z
k
)
=
c
o
v
(
x
~
k
+
1
|
k
,
z
~
k
+
1
|
k
|
z
k
)
P→Pk+1|k+1=cov(xk+1|zk+1)=cov(x~k+1|k+1|zk+1)=MSE(x^k+1|k+1|zk+1)
P
→
P
k
+
1
|
k
+
1
=
c
o
v
(
x
k
+
1
|
z
k
+
1
)
=
c
o
v
(
x
~
k
+
1
|
k
+
1
|
z
k
+
1
)
=
M
S
E
(
x
^
k
+
1
|
k
+
1
|
z
k
+
1
)
where
x~k|k=xk−x^k|k
x
~
k
|
k
=
x
k
−
x
^
k
|
k
x~k+1|k=xk+1−x^k+1|k
x
~
k
+
1
|
k
=
x
k
+
1
−
x
^
k
+
1
|
k
z~k+1|k=zk+1−z^k+1|k
z
~
k
+
1
|
k
=
z
k
+
1
−
z
^
k
+
1
|
k
代入状态方程与量测方程数据
x^k+1|k=E[xk+1|zk]=E[Fkxk+Gkuk+wk|zk]=Fkx^k|k+Gkuk
x
^
k
+
1
|
k
=
E
[
x
k
+
1
|
z
k
]
=
E
[
F
k
x
k
+
G
k
u
k
+
w
k
|
z
k
]
=
F
k
x
^
k
|
k
+
G
k
u
k
x~k+1|k=xk+1−x^k+1|k=Fkx~k|k+wk
x
~
k
+
1
|
k
=
x
k
+
1
−
x
^
k
+
1
|
k
=
F
k
x
~
k
|
k
+
w
k
Pk+1|k=E[x~k+1|kx~′k+1|k|zk]=FkPk|kF′k+Qk
P
k
+
1
|
k
=
E
[
x
~
k
+
1
|
k
x
~
k
+
1
|
k
′
|
z
k
]
=
F
k
P
k
|
k
F
k
′
+
Q
k
z^k+1|k=E[zk+1|zk]=E[Hk+1xk+1+vk+1|zk]=Hk+1x^k+1|k
z
^
k
+
1
|
k
=
E
[
z
k
+
1
|
z
k
]
=
E
[
H
k
+
1
x
k
+
1
+
v
k
+
1
|
z
k
]
=
H
k
+
1
x
^
k
+
1
|
k
z~k+1|k=zk+1−z^k+1|k=Hk+1x~k+1+vk+1
z
~
k
+
1
|
k
=
z
k
+
1
−
z
^
k
+
1
|
k
=
H
k
+
1
x
~
k
+
1
+
v
k
+
1
Sk+1=E[z~k+1|kz~′k+1|k|zk]=Hk+1Pk+1|kH′k+1+Rk+1
S
k
+
1
=
E
[
z
~
k
+
1
|
k
z
~
k
+
1
|
k
′
|
z
k
]
=
H
k
+
1
P
k
+
1
|
k
H
k
+
1
′
+
R
k
+
1
cov(xk+1,zk+1|zk)=E[x~k+1|z~k+1|zk]=Pk+1|kH′k+1
c
o
v
(
x
k
+
1
,
z
k
+
1
|
z
k
)
=
E
[
x
~
k
+
1
|
z
~
k
+
1
|
z
k
]
=
P
k
+
1
|
k
H
k
+
1
′
Define
Kk+1=cov(xk+1,zk+1|zk)S−1k+1=Pk+1|kH′k+1S−1k+1
K
k
+
1
=
c
o
v
(
x
k
+
1
,
z
k
+
1
|
z
k
)
S
k
+
1
−
1
=
P
k
+
1
|
k
H
k
+
1
′
S
k
+
1
−
1
Then
x^k+1|k+1=x^k+1|k+Kk+1z~k+1|k
x
^
k
+
1
|
k
+
1
=
x
^
k
+
1
|
k
+
K
k
+
1
z
~
k
+
1
|
k
Pk+1|k+1=Pk+1|k−Pk+1|kH′k+1S−1k+1Hk+1Pk+1|k
P
k
+
1
|
k
+
1
=
P
k
+
1
|
k
−
P
k
+
1
|
k
H
k
+
1
′
S
k
+
1
−
1
H
k
+
1
P
k
+
1
|
k
=(I−kk+1Hk+1)Pk+1|k...(1)
=
(
I
−
k
k
+
1
H
k
+
1
)
P
k
+
1
|
k
.
.
.
(
1
)
=Pk+1|k−Kk+1Sk+1K′k+1...(2)
=
P
k
+
1
|
k
−
K
k
+
1
S
k
+
1
K
k
+
1
′
.
.
.
(
2
)
An alternative form, which holds for an arbitrary gain matrix Kk+1 K k + 1 called the Joseph form, is
Pk+1|k+1=(I−Kk+1Hk+1)Pk+1|k(I−Kk+1Hk+1)′+Kk+1Rk+1K′k+1...(3) P k + 1 | k + 1 = ( I − K k + 1 H k + 1 ) P k + 1 | k ( I − K k + 1 H k + 1 ) ′ + K k + 1 R k + 1 K k + 1 ′ . . . ( 3 )
(3)式的优点:既保持了矩阵的对称性,也保证了矩阵为非负定矩阵。
KF 最终使用形式
状态和测量的预测值(time update):
x^k+1|k=Fkx^k|k+Gkuk
x
^
k
+
1
|
k
=
F
k
x
^
k
|
k
+
G
k
u
k
Pk+1|k=FkPk|kF′k+Qk
P
k
+
1
|
k
=
F
k
P
k
|
k
F
k
′
+
Q
k
z^k+1|k=Hk+1x^k+1|k
z
^
k
+
1
|
k
=
H
k
+
1
x
^
k
+
1
|
k
Sk+1=Hk+1Pk+!|kH′k+1+Rk+1
S
k
+
1
=
H
k
+
1
P
k
+
!
|
k
H
k
+
1
′
+
R
k
+
1
状态更新(measurement update):
Kk+1=Pk+1|kH′k+1S−1k+1
K
k
+
1
=
P
k
+
1
|
k
H
k
+
1
′
S
k
+
1
−
1
x^k+1|k+1=x^k+1|k+Kk+1(zk+1−z^k+1|k)
x
^
k
+
1
|
k
+
1
=
x
^
k
+
1
|
k
+
K
k
+
1
(
z
k
+
1
−
z
^
k
+
1
|
k
)
Pk+1|k+1=Pk+1|k−Kk+1Sk+1K′k+1
P
k
+
1
|
k
+
1
=
P
k
+
1
|
k
−
K
k
+
1
S
k
+
1
K
k
+
1
′
迭代形式需要初始化, x^0|0=E[x0|z0]=x¯0ofx0 x ^ 0 | 0 = E [ x 0 | z 0 ] = x ¯ 0 o f x 0 ,and the associated MSE initial MSE P0|0=MSE(x^0|0|z0)=cov(x0|z0)=P0 P 0 | 0 = M S E ( x ^ 0 | 0 | z 0 ) = c o v ( x 0 | z 0 ) = P 0