线性离散卡尔曼滤波器

线性离散卡尔曼滤波公式 两种数学推导方法的比较

1. 引言

卡尔曼滤波属于一种软件滤波方法，其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。从研究的历史来看，卡尔曼是首先研究的离散形式的卡尔曼滤波问题，所以最初的卡尔曼滤波算法被称为基本卡尔曼滤波算法，适用于解决随机线性离散系统的状态或参数估计问题。下面分别对比了离散线性卡尔曼滤波器的相关公式推导的两种方法。

2. 离散线性卡尔曼滤波器的直观数学推导

下面从直观角度来推导线性离散系统的卡尔曼滤波器，这是书中的推导方法。首先假设线性离散系统模型如下

,11,11

k k k k k k k

k k k k

x w z H x v x

----=Φ+Γ=+

其中，1k w -为过程噪声，k v 为观测噪声，k z 为第k 次的测量值，/ˆk k x

是k x 的最优线性估计，/1ˆk k x

-是k x 的一步预报估计。过程噪声1k w -和观测噪声k v 的统计特性为：

1[]0,(,)[]0,(,)(,)0

k ww k kj k vv k kj wv E w R k j Q E v R k j R R k j δδ-=====

初始状态0x 的统计特性为：

0000ˆ[],()E x x

Var x P == 并假定0x 与k w 和k v 均无关，则有：

00(0,)(,)0(0,)(,)0

T xw k T

xv k R k E x w R k E x v ====

据以上假设及条件，可得如下直观形式

/1,11/1/1/1//1/1ˆˆˆˆˆˆk k k k k k k k k k k k k k k k k k x

x z H x x

x K z --------=Φ==+

式中，/1/1ˆk k k k k z z z

--=-，k K 为待定的增益矩阵。 下面按照目标函数//[]T

k k k k J E x x =最小的要求，确定最优滤波增益矩阵。由述

定义可得

///1/1/1/1/1ˆˆ[][][]k k k k k k k k k k k k k k k k k k k k k k k k k k k

x x x

x K H x v H x x K H x v I K H x K v -----=-=-+-=-+=--

从而

///1/1/1/1/1/1{[]}{[]}[][][][]T T T T T

k k k k k k k k k k k k k k k k T T T T k k k k k k k k k k k k k k T T T T k k k k k k k k k k

x x I K H x K v x I K H v K I K H x x I K H K v x I K H I K H x v K K v v K ------=----=------+

由于/1k k x -是121,,...,k z z z -的线性函数，且测量误差是不相关的，所以根据向量知识有

/1/1[]0,[]0T T

k k k k k k E x v E v x --==

于是，滤波误差协方差矩阵为

////1[][][]T T T k k k k k k k k k k k k k k k P E x x I K H P I K H K R K -==--+

式中，/1/1/1[]T

k k k k k k P E x x ---=。

对上式同时加减一项：1/1/1/1[]T T

k k k

k k k k k k k k P H H P H R H P ----+变形如下： 1//1/1/1/1

1

/1/1/1()[()][]

T

T k k k k k k k k k k k k K K K T

T T k k k k k k k k

k k k k k

k P P P H H P H R H P K P H H P H R H P H R ---------=-++-++

要使/k k P 最小，则要使得上式中最后一项为0，即：

1/1/1()T T k k k k k k k k k K P H H P H R ---=+

此时，误差协方差矩阵为

1//1/1/1/1

/1

()[]T T

k k k k k k k k k k k k K K K k k k k P P P H H P H R H P I K H P ------=-+=-

下面计算预测误差协方差矩阵/1k k P -。为此，先计算预测误差

/1/1,11,11,11/1,11/1,11

ˆˆk k k k k k k k k k k k k k k k k k k k k k x x x

x w x x w --------------=-=Φ+Γ-Φ=Φ+Γ

因此有

/1/1,11/1,11,11/1,11,11/11/1,1,111/1,1,111,1,11/11,1

[][]T T

k k k k k k k k k k k k k k k k k k T T T T k k k k k k k k k k k k k k k T T T T k k k k k k k k k k k k k x x x w x w x x w x w w x w --------------------------------=Φ+ΓΦ+Γ=ΦΦ+ΓΦ+ΓΓ+ΦΓ

由于

,11/11,1,111/1

,1

[]0

[]0

T T

k k k k k k k T

T k k k k k k k E x w E w x

----------ΦΓ=ΓΦ

=

于是有

/1,11/1,11,11/1,11,11/11/1,1,111,1,11/1,1,11,1

[][]T

k k k k k k k k k k k k k k k k T T T T

k k k k k k k k k k k k k k T T k k k k k k k k k k k P E x w x w x x w w P Q ----------------------------=Φ+ΓΦ+Γ=ΦΦ+ΓΓ=ΦΦ+ΓΓ

至此，线性离散系统的卡尔曼滤波器的公式推导完毕。

3. 离散线性卡尔曼滤波器的正交投影法数学推导

与之前讨论的不同，此处讨论的离散线性系统中，考虑到1k w -和k v 可能是相关的，所以假设系统的形式为：

,111111k k k k k k k k x x B u w ------=Φ++Γ（1）

k k k k k z Hx D u ν=++（2）

（1）式为状态方程，（2）式为观测方程,式中x 为n 维状态向量；u 为m 维控制向量；w 为p 维动态噪声；Φ为n n ⨯阶状态转移矩阵；B 为n m ⨯阶矩阵;Γ为

n p ⨯阶矩阵；z 为q 维观测向量；ν为q 维观测噪声；H 为q n ⨯矩阵。D 为q m ⨯阶矩阵。

卡尔曼滤波器通过观测方程的关测量k z 的可测参数估计状态方程中状态量

k x 的状态值。将(2)式中k k D u 视为量测的系统误差项，{}k w 与{}k v 为零均值白噪声序列,由于状态方程中11k k B u --以及k k D u 的存在，可知在同一时刻k w 和k v 之间可能是相关的，这是与上面的直观推导过程中所假设的不同的。

由此，可知，模型的基本统计规律为：

{}0k E w =cov()[]T

k j k j

k kj w w E w w Q δ== {}0k E v =cov()[]T

k j k j

k kj v v E v v R δ== cov()[]T

k j k j k kj w v E w v S δ==（3）