线性离散卡尔曼滤波器

线性离散卡尔曼滤波公式 两种数学推导方法的比较1. 引言卡尔曼滤波属于一种软件滤波方法，其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。

从研究的历史来看，卡尔曼是首先研究的离散形式的卡尔曼滤波问题，所以最初的卡尔曼滤波算法被称为基本卡尔曼滤波算法，适用于解决随机线性离散系统的状态或参数估计问题。

下面分别对比了离散线性卡尔曼滤波器的相关公式推导的两种方法。

2. 离散线性卡尔曼滤波器的直观数学推导下面从直观角度来推导线性离散系统的卡尔曼滤波器，这是书中的推导方法。

首先假设线性离散系统模型如下,11,11k k k k k k kk k k kx 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,(,)(,)0k 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 xVar x P == 并假定0x 与k w 和k v 均无关，则有：00(0,)(,)0(0,)(,)0T xw k Txv 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 xx z H x xx K z --------=Φ==+式中，/1/1ˆk k k k k z z z--=-，k K 为待定的增益矩阵。

下面按照目标函数//[]Tk 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 kx x xx 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 Tk 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 kx 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 Tk 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[]Tk k k k k k P E x x ---=。

对上式同时加减一项：1/1/1/1[]T Tk k kk k k k k k k k P H H P H R H P ----+变形如下： 1//1/1/1/11/1/1/1()[()][]TT k k k k k k k k k k k k K K K TT T k k k k k k k kk k k k kk 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 Tk 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 xx 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 Tk 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[]0T Tk k k k k k k TT 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[][]Tk k k k k k k k k k k k k k k k T T T Tk 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()[]Tk j k jk kj w w E w w Q δ== {}0k E v =cov()[]Tk j k jk kj v v E v v R δ== cov()[]Tk j k j k kj w v E w v S δ==（3）这里kj δ是克罗内科函数,即:初始状态的统计特性为00E x u ⎡⎤=⎣⎦{}00000var ()()Tx E x x P μμ=--=（4） 此时0x 和{}k w {}k v 不相关。

下面利用正交投影法推导线性离散系统的卡尔曼滤波公式：首先将动态方程变形，使得动态噪声和量测噪声不相关。

,111111111111()k k k k k k k k k k k k k k k x x B u w J z H x D u ν------------=Φ++Γ+---（5） 令：*,1,111k k k k k k J H ----Φ=Φ-（6） *1111k k k k w w J υ----=Γ-（7）**,111111111()k k k k k k k k k k kx x B u J z D u w ---------=Φ++-+（8） 将111111()k k k k k k B u J z D u ------+-视为新控制项，则*cov()()()Tk j k k k k j k k k k kq w E w J S J R νννδ=Γ-=Γ-（9）取1k k k k J S R -=Γ（10）则动态噪声与量测声不相关。

然后求取最优线性测量的估计值设基于钱k-1次测量的测量向量集合11121...Tk T T Tk Z z z z --⎡⎤=⎣⎦，记y 是x 在z 上的投影为ˆ(\)y Ex z =则 1111ˆ(\)ˆk k k x Ex Z ---=（11） 1/11ˆ(\)ˆk k k k x Ex Z --=（12） 将（9）（11）（7）代入到（12）里得*1/1,111111111*,1/1111111*,1/1111111*,1/111111111ˆ(\)()ˆ()ˆ()ˆ()ˆˆ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 k k k k k k k k k k k k k k k k k x E x Z B u J z D u x B u J z D u x B u J z D u x B u J z D u H x ------------------------------------=Φ++-=Φ++-=Φ++-=Φ++--（13）接下来就是求最优线性测量值1/1111/1ˆ(\)ˆˆ()\k k k k k k k k k k k k k k kz Ez Z E H x D u Z H x D u ν----=⎡⎤=++⎣⎦=+（14） 之后找到k z 与11k Z -的新息成分，由上式可以得到/1/1/1ˆk k k k k k k k k z z z H x ν---=-=+（15） /1/1/1/1/1()()()T Tk k k k k k k k k k k k Tk k k k kE z z E H x H x H P H R νν-----⎡⎤=++⎣⎦=+（16）/1/1/1/1/1()[()]T T Tk k k k k k k k k k k k kE x z E x H x P H ν-----=+=（17） 求取ˆk x 的递推公式1111/1/1/1/1/1/1/1ˆ(\)ˆˆ(\)(())(())()ˆˆk k k k T T k k k k k k k k k k k k k k k k k k k x Ex Z E x Z E x z E z z z x K z D u H x ---------==++=+--（18） 其中1/1/1()T T k k k k k k k k k K P H H P H R ---=+（19）之后求取误差方差的递推公式：/1/1ˆk k k k k k k k x x x x K z --=-=-（20）**/1,111k k k k k kx x w -=---=Φ+（21） 分别对上述两个等式的两边求方差阵，由（7）（8）（10）式可得****/1,11,111,1111,111111111()()T Tk k k k K k k k k TTT k k k k k k k k k k k k k k k P P Ew w J H P J H QJ R J -------------------=ΦΦ+=Φ-Φ-+ΓΓ-（22）由（16）（17）（19）得到//1/1/1/1/1()()T T T Tk k k k k k k k k k k k k k k k k k k k k k k P P K H P H R K P H K K H P I K H P -----=++--=-综上，可知预测方程为/1,1111111111()ˆˆˆk k k k k k k k k k k k k x x B u J z D u H x -----------=Φ++--/1/1ˆk k k k k k k z H x D u --=+ 滤波方程为//1/1()ˆˆˆk k k k k k k k k k k x x K z D u H x --=+--增益矩阵1/1/1()T T k k k k k k k k k K P H H P H R ---=+方差阵/1,1111,111111111()()T T T k k k k k k k k k k k k k k k k kP J H P J H Q J R J --------------=Φ-Φ-+ΓΓ-11111k k k k J S R -----=Γ //1()k k k k k k P I K H P -=-于是，线性离散系统的卡尔曼滤波器的公式推导完毕。