非线性系统中卡尔曼滤波的一种新线性化方法

δxk +1 = <δxk + Γk Wk
(17)
同时定义 :
δZk +1 = Zk +1 - h ( ^xk , k + 1)
(18)
将式 (7) 中的观测方程在本时刻状态最优估计处
按照泰勒级数展开 :
Zk +1
=
h ( ^xk , k + 1)
+
5 5
h x
( xk +1 x =^xk
^xk)
+
4 应用实例
例 1 对于 GPS 双差静态定位 , 使用静态卡 尔曼滤波 (即序贯平差) , 其原始状态方程和观测 方程为 :
xk +1 = xk
Zk +1 = h ( xk +1 , k + 1) + Vk +1 其中状态矢量为三维空间坐 标 未 知 量 ( X , Y , Z) T ; h 为 GPS 定位模型函数 。
1) 采用标称状态线性化方式 ,即可将测站的近 似坐标设为标称状态 x0 ,则部分滤波方程组为 :
δ^xk +1/ k =δ^xk/ k δZk +1 = Zk +1 - h ( x0 , k + 1)
© 1994-2008 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net
化的精度效果和广义卡尔曼滤波基本相同 。
这种线性化方法的优点是其线性化点与观测
对象的运动状态无关 , 观测对象的动态噪声不会 影响到线性化的精度 。更为重要的是 , 在最小二
乘估计中 ,由于不顾及观测对象的运动状态 ,其线
性化常在前一步的估计结果处进行 , 这样本文的 线性化方式就和最小二乘线性化一致 。
δ^xk +1/ k +1 =δ^xk +1/ k + Kk +1 (δZk +1 - Hδ^xk +1/ k)
2) 采用广义卡尔曼滤波线性化 , 则部分滤波
方程组为 : δ^xk +1/ k =δ^xk/ k = 0
δZk +1 = Zk +1 - h ( ^xk +1/ k , k + 1) δ^xk +1/ k +1 = Kk +1δZk +1
Vk +1 = h ( ^xk , k + 1) + Hδxk +1 + Vk +1 (19)
得:
δZk +1 = Hδxk +1 + Vk +1
(20)
结合式 (17) 、式 ( 20) 即可得到按照前一步状
态线性化的状态方程和量测方程 :
δxk +1 = <δxk + Γk Wk δZk +1 = Hδxk +1 + Vk +1
例 2 对于 GPS 双差动态定位 , 如状态方程 使用匀速模型 , 则卡尔曼滤波的原始状态方程和 观测方程为 :
xk +1 = Φk +1/ k x k + Γk Wk
Zk +1 = h ( xk +1 , k + 1) + Vk +1 其中状态为三维空间位置和速度未知量 ( X , Y , Z , V X , V Y , V Z) T ,其状态转移矩阵为 :
(21)
根据式 (21) 可以推导出相应的线性滤波公式为 :
δ^xk +1/ k = <k +1 , kδ^xk/ k
(22)Baidu Nhomakorabea
Pk +1/ k = <k +1 , k Pk/ k<Tk +1 , k + ΓkQkΓTk
(23)
δ^xk +1/ k +1 =δ^xk +1/ k + Kk +1 (δZk +1 - Hδ^xk +1/ k)
第 29 卷 第 4 期 2004 年 4 月
武 汉 大 学 学 报 ·信 息 科 学 版 Geomatics and Information Science of Wuhan University
Vol. 29 No. 4 Apr. 2004
文章编号 :167128860 (2004) 0420346203
卡尔曼滤波在测量领域的应用一般都存在线性 化的问题 。卡尔曼滤波的线性化目前主要有按系统 标称状态线性化和按系统状态最优估计线性化 (也 称广义卡尔曼滤波) 两种方式。在静态定位领域 ,由 于观测目标状态不变 ,因而标称状态的误差不会随 滤波过程增大 ,这两种线性化方法都易于使用。但 在动态定位领域 ,由于状态方程中动态噪声的原因 , 系统的标称状态和实际状态容易相差过大 ,按照标 称状态线性化方式的线性化就会有较大误差 ,从而 影响滤波精度 。广义卡尔曼滤波按照系统状态最优 估计进行线性化 ,其线性化精度可以保证 ,但是其线 性化是在本时刻预报的基础上进行的 ,即线性化时 必须确定定位点的动态模型 ,而在卡尔曼滤波的实 际应用中 ,如采用多模型的自适应滤波方法[1] ,一旦 动态模型改变 ,系统就必须重新线性化 ,不便进行数 据处理。同时 ,此线性化方法和测量界常用的最小 二乘线性化方法不同 ,后者经常在卡尔曼滤波中被 用来作各种辅助处理 ,如成果检核和精度分析等 ,数 据处理也不便。因此 ,本文提出了一种新的线性化 方法 ,使用前一步最优估计值进行线性化 ,将最小二 乘和卡尔曼滤波的线性观测方程统一 ,从而便于数 据处理 ,也能保证精度 ,在空间状态变化不大的情况 下 ,线性化精度和广义卡尔曼滤波类似。
3) 采用前一步估计状态线性化 , 由于 ^xk = ^xk - 1 ,即δ^xk/ k = 0 ,所以其滤波方程组同广义卡尔 曼滤波线性化形式一致 。
对于静态形式 ,标称状态 x0 的取值如果偏差不 大 ,则可以满足定位需求 ,三者定位结果精度相同 , 如果 x0 的偏差较大 ,采用方式 1) 则会产生较大的线 性化误差。在工程实践中 ,由于 x0 易于取得近似真 值 ,所以在实际数据处理中采取任一种方法均可。
(11)
Pk +1/ k = <k +1 , k Pk/ k<Tk +1 , k + ΓkQkΓTk (12)
δ^xk +1/ k +1 =δ^xk +1/ k + Kk +1δZk +1
(13)
式 (13) 可以进一步表示为 : δ^xk +1/ k +1 = Kk +1 [ Zk +1 - h ( f ( ^xk) ) ] (14)
(3)
Kk +1
=
Pk +1/
k
H
T k +1
(
Hk +1
Pk +1/
k Hk +1
+
Rk +1) - 1
(4)
^xk +1/ k +1 = ^xk +1/ k + Kk +1 ( Zk +1 - H^xk +1/ k) (5)
Pk +1/ k +1 = ( I - Kk +1 Hk +1) Pk +1/ k ( I -
1 经典方程
对于线性系统 :
xk +1 = Φk +1 , k xk + Γk Wk (1)
Zk +1 = Hxk +1 + Vk +1
经典线性离散卡尔曼滤波方程为 :
^xk +1/ k = Φk +1 , k^xk/ k
(2)
Pk +1/ k = Φk +1 , k Pk/ kΦTk +1 , k + ΓkQkΓTk
(24) 从形式上看 ,此滤波公式和按照标称状态线性
化的公式相同 ,但由式(15) 的定义可以得知 ,由于 ^xk 的误差不会随着滤波时间的增长而愈来愈大 ,因而 ,
这种线性化方式不会像按标称状态线性化那样会导
致线性化误差过大的问题。同时 ,式(24) 中的滤波 结果δ^xk + 1/ k + 1为一个滤波周期中状态的变化量 ,在 状态变化缓慢的情况下 ,其数值不会太大 ,此时线性
(8)
Pk +1/ k = <k +1 , k Pk/ k<Tk +1 , k + ΓkQkΓTk
(9)
δ^xk +1/ k +1 =δ^xk +1/ k + Kk +1 (δZk +1 - Hδ^xk +1/ k)
(10)
式中 , < = 5 f / 5 x , H = 5 h/ 5 x 。 由于式 (8) 中的δ^xk/ k是根据标称的物理过程
给出的 ,如果实际物理过程和标称物理过程相差 过大 ,会导致δ^xk/ k 数值过大 , 线性化误差也会相 应增大 。
围绕状态估计值线性化的滤波方法即为广义
卡尔曼滤波 。在广义卡尔曼滤波中 , 对应卡尔曼
滤波基本方程的式 (2) 、式 (3) 、式 (5) 为 :
δ^xk +1/ k = <k +1 , kδ^xk/ k = 0
© 1994-2008 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net
第 4 期
孙红星等 :非线性系统中卡尔曼滤波的一种新线性化方法
347
如下形式 :
δ^xk +1/ k = <k +1 , kδ^xk/ k
将式 (7) 中的状态方程在前一步状态最优估
计处按泰勒级数展开 ,并取一次形式 :
xk +1
= f ( ^xk - 1 , k -
1)
+
5 5
f x
( xk ^ x = xk- 1
^xk - 1)
+
Γk W = ^xk/ k - 1 + <( xk - ^xk - 1) + Γk W
(16)
将 ^xk/ k - 1改写成 ^xk ,则由式 (16) 可推得 :
348
武 汉 大 学 学 报 ·信 息 科 学 版
2004 年
δ^xk +1/ k +1 =δ^xk/ k + Kk +1 (δZk +1 - Hδ^xk/ k) 2) 采用广义卡尔曼滤波线性化 , 则部分滤波 方程组为 :
δZk +1 = Zk +1 - h ( ^xk , k + 1) δ^xk +1/ k +1 = Kk +1δZk +1
在线性化时 ,卡尔曼滤波基本方程的式 (4) 、式 (6) 变化较小 ,较易写出 ,本文主要针对线性化中的 式(2) 、式(3) 、式(5) 进行讨论。目前使用的两种线性 化为按标称状态线性化和按照状态估计值线性化 。
按标称状态线性化后 ,式 (2) 、式 (3) 、式 (5) 有
收稿日期 :2004201208 。 项目来源 :国家自然科学基金资助项目 (40023004) 。
化方法不同 ,所得观测方程亦不同 ,不便于数据处
理 。本文提出了在上一步的定位结果的基础上进
行线性化的方法 , 可将最小二乘和卡尔曼滤波的
线性观测方程统一 ,从而便于数据处理 ,也能保证
精度 。以下进行理论推导 。
定义 :
δxk +1 = xk +1 - ^xk
(15)
式中 , ^xk 为本时刻的估计值 。
Kk +1 Hk +1) T + Kk +1 Rk +1 KTk +1
(6)
2 目前的两种线性化方法[2 ,3 ]
在一般的工程实践中 ,滤波方程的状态方程 或量测方程常是非线性方程 ,最常见的是系统噪 声为线性的非线性方程 :
xk +1 = f ( xk , k) + Γk Wk (7)
Zk +1 = h ( xk +1 , k + 1) + Vk +1 经典滤波方程式 (2) ～式 (6) 并不能直接使用 ,要 推导出相应的线性公式 。
100 t00
0100 t0
Φk +1 , k =
0 0
0 0
1 0
0 1
0 0
t 0
000010
000001 式中 , t 为采样间隔时间 。观测方程同静态定位 相同 。
1) 采用标称状态线性化方式 ,可以根据状态 方程算得标称状态 xkn + 1 ,则部分滤波方程组为 :
δ^xk +1/ k = Φk +1/ kδ^xk/ k δZk +1 = Zk +1 - h ( xkn+1 , k + 1)
文献标识码 :A
非线性系统中卡尔曼滤波的一种新线性化方法
孙红星1 李德仁2
(1 武汉大学遥感信息工程学院 ,武汉市珞喻路 129 号 ,430079) (2 武汉大学测绘遥感信息工程国家重点实验室 ,武汉市珞喻路 129 号 ,430079)
摘 要 :针对测量领域非线性系统卡尔曼滤波的线性化 ,在分析两种传统线性化方式的基础上 ,提出了一种新 的基于最优估计值的线性化方式 。 关键词 :卡尔曼滤波 ; 线性化 ; GPS 中图法分类号 : P228. 41 ; P207
由于式 (11) 中的δ^xk/ k总为零 ,线性化误差仅 受上一步估计结果精度及下一步预报精度的影
响 ,所以精度较高 。
3 按前一步状态最优估计线性化
最小二乘线性化中 , 由于不顾及观测点的运
动状态 ,其线性化一般都是采用上一步的定位结
果 。这样 ,如果在数据处理中同时使用此两种估
计方法 ,由于最小二乘和广义卡尔曼滤波的线性