线性离散卡尔曼滤波器

线性离散卡尔曼滤波公式 两种数学推导方法的比较 1 引言 卡尔曼滤波属于一种软件滤波方法 其基本思想是 以最小均方误差为最 佳估计准则 采用信号与噪声的状态空间模型 利用前一时刻的估计值和当前 时刻的观测值来更新对状态变量的估计 求出当前时刻的估计值 算法根据建 立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计 从 研究的历史来看 卡尔曼是首先研究的离散形式的卡尔曼滤波问题 所以最初 的卡尔曼滤波算法被称为基本卡尔曼滤波算法 适用于解决随机线性离散系统 的状态或参数估计问题 下面分别对比了离散线性卡尔曼滤波器的相关公式推 导的两种方法 2 离散线性卡尔曼滤波器的直观数学推导 下面从直观角度来推导线性离散系统的卡尔曼滤波器 这是书中的推导方 法 首先假设线性离散系统模型如下 11 11k kkk kk k kkkk xw zH xv x 其中 为过程噪声 为观测噪声 为第 k 次的测量值 是的最 1k w k v k z k kx k x 优线性估计 是的一步预报估计 过程噪声和观测噪声的统计特 1 k kx k x 1k w k v 性为 1 0 0 0 kwwkkj kvvkkj wv E wRk jQ E vRk jR Rk j 初始状态的统计特性为 0 x 0000 E xx Var xP 并假定与和均无关 则有 0 x k w k v 0 0 0 0 0 0 T xwk T xvk RkE x w RkE x v 据以上假设及条件 可得如下直观形式 1 11 1 1 1 1 1 k kk kkk k kkk k k kk kkk k xx zH x xxK z 式中 为待定的增益矩阵 1 1 k kkk k zzz k K 下面按照目标函数最小的要求 确定最优滤波增益矩阵 由 T k kk k JE xx 述定义可得 1 1 1 1 1 k kkk k k kkkkkkk k k kkkk kk kkk kkk xxx xKH xvH x xKH xv IK HxK v 从而 1 1 1 1 1 1 TTTTT k kk kkkk kkkk kkkkk TTTT kkk kk kkkkkk kkk TTTT kkk kkkkkkk xxIK HxK vxIK Hv K IK HxxIK HK v xIK H IK Hxv KK v v K 由于是的线性函数 且测量误差是不相关的 所以根据向量知 1k k x 121 k z zz 识有 1 1 0 0 TT k kkkk k E xvE v x 于是 滤波误差协方差矩阵为 1 TTT k kk kk kkkk kkkkkk PE xxIK HPIK HK R K 式中 1 1 1 T k kk kk k PE xx 对上式同时加减一项 变形如下 1 1 1 1 TT k kkkk kkkkk k PHH PHRH P 1 1 1 1 1 1 1 1 1 TT k kk kk kkkk kkkKK K TTT kk kkkk kkkkk kkk PPPHH PHRH P KPHH PHRH PHR 要使最小 则要使得上式中最后一项为 0 即 k k P 1 1 1 TT kk kkkk kkk KPHH PHR 此时 误差协方差矩阵为 1 1 1 1 1 1 TT k kk kk kkkk kkkKK K kkk k PPPHH PHRH P IK HP 下面计算预测误差协方差矩阵 为此 先计算预测误差 1k k P 1 1 11 11 11 1 11 1 11 k kkk k k kkk kkk kkk k kkkk kk xxx xwx xw 因此有 1 1 11 1 11 11 1 11 11 11 1 1 111 1 1 111 1 11 11 1 TT k kk kk kkkk kkk kkkk kk TTTT k kkkkkk kk kkkkk k TTTT k kkkk kk kkkkk k xxxwxw xxwx wwxw 由于 11 11 1 111 1 1 0 0 TT k kkkkk k TT k kkkkk k Exw Ewx 于是有 1 11 1 11 11 1 11 11 11 1 1 111 1 11 1 1 11 1 T k kk kkkk kkk kkkk kk TTTT k kkkkkk kk kkkk k TT k kkkk kk kkk k PExwxw xxww PQ 至此 线性离散系统的卡尔曼滤波器的公式推导完毕 3 离散线性卡尔曼滤波器的正交投影法数学推导 与之前讨论的不同 此处讨论的离散线性系统中 考虑到和可能是 1k w k v 相关的 所以假设系统的形式为 1 111111kk kkkkkk xxBuw 2 kkkkk zH xDu 1 式为状态方程 2 式为观测方程 式中为维状态向量 为维控制xnum 向量 为维动态噪声 为阶状态转移矩阵 B 为阶矩阵 为wp nn nm 阶矩阵 z 为维观测向量 为维观测噪声 H 为矩阵 D 为np q qqn 阶矩阵 qm 卡尔曼滤波器通过观测方程的关测量的可测参数估计状态方程中状态量 k z 的状态值 将 2 式中视为量测的系统误差项 与为零均值白噪 k x kk Du k w k v 声序列 由于状态方程中以及的存在 可知在同一时刻和之间 11kk Bu kk Du k w k v 可能是相关的 这是与上面的直观推导过程中所假设的不同的 由此 可知 模型的基本统计规律为 0 k E w cov T kjkjkkj w wE w wQ 0 k E v cov T kjkjkkj v vE v vR 3 cov T kjkjkkj wvE wvS 这里是克罗内科函数 即 kj 初始状态的统计特性为 4 00 E xu 00000 var TxExxP 此时和不相关 0 x k w k v 下面利用正交投影法推导线性离散系统的卡尔曼滤波公式 首先将动态方程变形 使得动态噪声和量测噪声不相关 5 111111111111 kk kkkkkkkkkkkkk xxBuwJzHxDu 令 6 1 111k kk kkk JH 7 1111kkkk wwJ 8 111111111 kk kkkkkkkkk xxBuJzDuw 将视为新控制项 则 111111 kkkkkk BuJzDu 9 cov T kjkkkkjkkkkkq wEwJSJ R 取 10 1 kkkk JS R 则动态噪声与量测声不相关 然后求取最优线性测量的估计值 设基于钱 k 1 次测量的测量向量集合 记 y 是 x 在 z 1 1121 T kTTT k Zz zz 上的投影为则 yE xz 11 1 111 k kk xE xZ 12 1 11 k k kk xE xZ 将 9 11 7 代入到 12 里得 13 1 1 111111111 1 1111111 1 1111111 1 111111111 k k kk kkkkkkkk k kk kkkkkkk k kk kkkkkkk k kk kkkkkkkkk xE xZBuJzDu xBuJzDu xBuJzDu xBuJzDuHx 接下来就是求最优线性测量值 14 1 11 1 1 1 k k kk k kkkkk kk kkk zE zZ EH xDuZ H xDu 之后找到与的新息成分 由上式可以得到 k z 1 1 k Z 15 1 1 1 k kkk kkk kk zzzH x 16 1 1 1 1 1 TT k kk kkk kkkk kk T kk kkk E zzEH xH x H PHR 17 1 1 1 1 1 TTT k kk kk kkk kkk kk E xzE xH xPH 求取的递推公式 kx 18 1 11 1 1 1 1 1 1 1 1 k kk kTT kk kk kk kk kk k k kkkkkkk xE xZ E xZE xzE zzz xK zDuH x 其中 19 1 1 1 TT kk kkkk kkk KPHH PHR 之后求取误差方差的递推公式 20 1 1 kkkk kkk k xxxxK z 21 1 111k kk kkk xxw 分别对上述两个等式的两边求方差阵 由 7 8 10 式可得 1 11 111 1111 111111111 TT k kk kKk kkk TTT k kkkkk kkkkkkkkk PPEww JHPJHQJRJ 22 由 16 17 19 得到 1 1 1 1 1 TTTT k kk kkkk kkkkk kkkkkk k kkk k PPK H PHR KPH KK H P IK H P 综上 可知预测方程为 1 1111111111 k kk kkkkkkkkkk xxBuJzDuHx 1 1 k k kk kkk zH xDu 滤波方程为 1 1 k kk kkkkkkk k xxK zDuH x 增益矩阵 1 1 1 TT kk kkkk kkk KPHH PHR 方差阵 1 1111 111111111 T TT k kk kkkkk kkkkkkkkk PJHPJHQJRJ 1 1111kkkk JSR 1