《误差理论与测量平差基础教学课件》第5讲 kalman滤波 25页

常用模型方程： Xk k,k1Xk1Wk
Lk AkXk ek
k ,k 1 ——从k-1时刻到k时刻的状态转移矩阵
A k ——系数矩阵
W k ——系统误差向量（也称为模型噪声）
e k ——观测误差向量（也称为观测噪声）
E(ek)0 D (ek)2Q k2P k 1
2）Kalman滤波的计算过程是一个不断“预测—修正”的过程， 求解时不需存储大量数据，非常适于实时处理，计算机编程实 现。但计算量较大。
3) 滤波增益矩阵与观测无关，可预先离线算出，从而可以减少 在线实时计算工作量。
第五讲 动态线性系统的 Kalman滤波
协方差阵为
X k
T k 1 X ˆk 1 k 1
W k 1
误差方程为
VXk Xk Xˆk
PX k
Vk AkX ˆkLk P k
k1 k,k1
（1）
Kalman滤波的目标函数：
V X T kP XkV XkV k TP kV kmin
第五讲 动态线性系统的 Kalman滤波
(k 1 ) (k ) T (k )
(k 1 ) (k ) T(k u )
称为距离预测方程 称为速度预测方程
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 1、Kalman滤波数学模型 矩阵形式 X X 1 2((k k 1 1 )) 1 0T 1 X X 1 2((k k)) u 10 (k)
不失一般性，写作 X (k 1 ) k 1 ,kX (k ) W (k ) （1）
（1）式称为系统状态方程，X为信号，W为噪声向量
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 1、Kalman滤波数学模型
为了估计信号，观测了n个含有噪声的观测值，其观测方程为
L1(k) a1X1(k) e1(k) L2 (k) a2 X 2 (k) e2 (k) Ln (k) an X t (k) en (k)
二、 Kalman滤波原理
2、Kalman滤波解
（1）参数平差法 V X T kP X kV X kV k TP kV kmin
求导：
d dX ˆk2VX TkPXk 2VkTPkA k0
PXkVXkA k TP kV k0
VXk Xk Xˆk
Vk AkX ˆkLk
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 1、Kalman滤波数学模型
雷达跟踪问题：
设k时刻，雷达与飞行器的距离 R(k)
而T秒后的k＋1时刻，雷达与飞行器的距离 R(k1)
若飞行器的径向运动速度为 (k) R 为平均距离。
则有 (k 1 ) (k ) T (k )
X ˆk (IKkA k)Xk
Xk k1X ˆk1
K k P X k 1 A k T (P k 1 A k P X k 1 A k T ) 1
X k
T k 1 X ˆk 1 k 1
W k 1
滤波增益阵
第五讲 动态线性系统的 Kalman滤波
L (k ) A (k )X (k ) e (k ) 观测方程
动态线性系统的Kalman滤波的实质，就是利用观测值
L 1(k)L ,2(k) , ,L n(k)，根据状态方程和观测方程求定j时刻状
态向量
X
的最佳估值
j
Xˆ ( j / k)
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 1、Kalman滤波数学模型
第五讲：动态线性系统的 Kalman滤波
（Kalman Filtering of Dynamic Linear System）
一、背景 二、 Kalman滤波原理 三、 Kalman滤波算法的特性与改进
第五讲 动态线性系统的 Kalman滤波
一、背景——Kalman滤波理论的发展
滤波——是指从混合在一起的诸多信号中提取出所需信号的过程。 滤波估计的发展经历了三个阶段：
则有： X X 1(2k(k1)1)XX 1(2k(k ) )Tu1X 2((kk)) 矩阵形式 X X 1 2((k k 1 1 )) 1 0T 1 X X 1 2((k k)) u 10 (k)
法方程： ( P X k A k T P k A k )X ˆk (A k T P k L k P X k X k )
Kalman滤波解 X ˆk ( P X k A k T P k A k ) 1 ( A k T P k L k P X k X k )
X ˆ k ( P X k A k T P k A k ) 1 ( A k T P k L k P X k k ,k 1 X ˆ k 1 )
求极值
X ˆk2VX TkPXk 2KTAk0 V k 2VkTPk2KT0
Байду номын сангаас
VXk PXk1AkTK
Vk Pk1K
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 2 、Kalman滤波解
（2）条件平差法
因为：
VXk Xk Xˆk
所以：
Xˆk XkVXk XkPXk1AkTK
P X ˆk 1(P X kA k TP kA k)1
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 2 、Kalman滤波解
（2）条件平差法
条件方程： AkX ˆkLkVk0
（2）
目标函数： V X T k P X k V X k V k T P k V k 2 K T ( A k X ˆ k L k V k ) m
VXk PXk1AkTK
Vk Pk1K
将 Xˆ k 和 V k 代入条件方程：AkX ˆkLkVk0
法方程： (P k 1 A k P X k 1 A k T )K (A k X k L k )
K (P k 1 A k P X k 1 A k T ) 1 (A k X k L k )
写成矩阵形式 L (k ) A (k )X (k ) e (k ) （2）
（2）式称为观测方程，或测量模型
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 1、Kalman滤波数学模型 动态线性系统的模型方程
X (k ) k ,k 1 X (k 1 ) W (k ) 系统状态方程
二、 Kalman滤波原理 2 、Kalman滤波解
说明：
在一个滤波周期内，Kalman滤波具有两个明显的信息 更新过程：时间更新过程和观测更新过程。
时间更新：仅使用与系统动态特性有关的信息
Xk k1X ˆk1
X k
T k 1 X ˆk 1 k 1
W k 1
观测更新：正确、合理地利用观测信息
设：
K k P X k 1 A k T (P k 1 A k P X k 1 A k T ) 1
滤波增益阵
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 2 、Kalman滤波解
（2）条件平差法 Kalman滤波解的通用式
X ˆkX kK k(L kA kX k)
（4）求Kalman滤波解
X ˆkXkKkek X ˆk XkK kA kXk
第五讲 动态线性系统的 Kalman滤波
三、 Kalman滤波算法的特性与改进 1．Kalman滤波算法的特性
1）Kalman滤波模型具有广泛适应性。不仅适用于平稳序列滤 波，而且特别适用于非平稳序列以及高斯－马尔可夫序列滤波。
一、背景——Kalman滤波理论的应用
阿波罗登月计划和C—5A飞机导航系统的设计。 惯性导航、制导系统的设计； 全球定位系统的设计； 目标跟踪处理与辨识； 通信与信号处理； 随机最优控制问题、故障诊断； 组合导航系统的设计； 多传感器信息融合系统的设计与估计； ……
（1）计算状态向量的预报值及其协方差阵
Xk k1X ˆk1
X k
T k 1 X ˆk 1 k 1
W k 1
（2）求预测残差向量：ek LkAkXk
（3）计算滤波增益阵 K k 及 Kkek
K k X kA k T ( k A k X kA k T ) 1
E(Wk)0
D (W k)2Q W k2P W k 1
Wk与Wk-1，ek与 ek-1，Wk与ek均不相关,即为高斯白噪声。
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 2、 Kalman滤波解
设：
Xk k1X ˆk1为状态预报向量，作为虚拟观测值，
T k 1 X ˆk 1 k 1
W k 1
修正（Correction）：
X ˆ(k )K k(L kA kX (k )) X ˆ(k)X (k) X ˆ(k)
X ˆk KkAkXk
X ˆk Xk X ˆk
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 3、Kalman滤波计算步骤
称为距离预测方程
对于加速度 u(k ) ，也有如下加速度方程
(k 1 ) (k ) T(k u ) 称为速度预测方程
第五讲 动态线性系统的 Kalman滤波
二、 Kalman滤波原理 1、Kalman滤波数学模型
定义信号X（k），包含距离分量和径向速度分量
即设： X1(k)(k) X2(k)(k) 并令：u1(k)T(u k)
第五讲 动态线性系统的 Kalman滤波
一、背景——Kalman滤波理论的发展