组合导航系统新息自适应卡尔曼滤波算法_卞鸿巍

0 , cov[ Wk , Wj ] =Qk δkj , E[ Vk ] =0 , cov [ Vk , Vj ] =
Rk δkj
,
δkj
=
1 0
, ,
k k
=j ≠j
.
以新息方式列写标准卡尔曼
滤波算法为 :
^ Xk+1 =Υ^ X k
(1)
P -k+1 =ΥP k+ΥT +Qk
(2)
Kk
=P
k
HkT C
k
∑ tr
C -1 vj
-
vTj C
C - 1
vj
v - 1
vj j
=
j =j0
k
∑
tr
C
v
1j (Cv
j
-
vj
v
T j
)C
v
1 j
=0 (16)
j =j0
根据 1. 1 节的假设(3), 由式(16)可得式(8), 即
C^v k 是 Cv k 的极大似然最优估计.
2 GPS/INS 组合导航系统及系统模型
2. Dept . of Elect rical Eng . , Nav al U niv. of Eng. , Wuhan 430033)
Abst ract :T his paper presented an INS /GPS adaptive navig ation sy st em f or marine applica tion. T he G PS wi th tw o ant ennas providi ng vessel’ s al tit ude i s selected as the auxi lia ry navig atio n system to f use w i th INS to obtain bet ter estim ation accuracy o f INS e rrors. A novel i nnovati on-based adaptive estim ation (IAE) Kalm an fi ltering approach w as propo sed t o solve t he deg radation of the i nt egrat ed navig ation sy st em caused by GPS unstable measurement disturbs. T he developed adaptive Kalm an fi lter is based o n t he maxim um likelihoo d crit erio n fo r the proper com put ation o f the fi lter innovatio n cov ariance and hence t he filt er gain. T he simulation resul ts sho w t hat the adaptive Kalman fi lter out perf orms the st andard Kalm an filt er wi th bet ter accuracy , robust ness and less co mput atio n. It is demo nst rated that this pro po sed approach i s a vali d soluti on f or the unknow n chang ing measurement noi se exi ted i n Kalm an filt er. Key words:iner tial navigati on sy stem (INS);glo bal po sitio ning sy st em (GPS);integ rat ed navig ation sy st em ;adapti ve Kalman fil ter ;innovat ion-based adapti ve est imatio n(IAE)
C-v
1 k
vk]
(11)
若要求式(11)取最大值 , 对 k 时刻之前 k - j 0 +1 组
数据进行累加并忽略常数项 , 则式(11)需满足
k
k
∑ ∑ ln |Cvj |+
vTj
C
-1 vj
vj
=min
j =j0
j =j0
(12)
根据极大似然估计准则 , 即满足 : p / r =0 , 对式
(2) 各段噪声变化强度未知 , 之间互不相关 ; (3) 在有限记忆长度为 N 的滑动采样区间 , 新
息采样具备遍历性.
记 C^v k 为 C v k 在长度为 N 的滑动采样的最优估 计值 , 则有
k
∑ ^Cv k
=1 N
vjv
T j
j =j0
(8)
将式(8)代入式(3), 即得改进的 IAE 自适应卡尔曼
度函数表示在 r 条件下 Z 的条件概率密度函数 :
p(Z |r)k =
1 (2π)m |Cv
k
e|
1 2
vTk
C-v
1k v
k
(10)
式中 :m 为观测量数目 ;Cv k 为 k 时刻理想的新息方
差. 对式(10)取对数 , 得
ln
p(Z |r)k
=-
1 2
[
m
ln(2π) +
ln
|Cv k
|+vkT
-1 vk
(3)
X^k =^ X -k +Kk vk
(4)
P
+ k
=[
I
-
K kH k]
P
k
(5)
式中 :vk 为滤波器的新息状态 ;Cv k 为新息方差 ,
vk =Zk - HkX^-k
(6)
Cv k
=Hk
P
k
H
T k
+Rk
(7)
对于外部量测噪声的变化情况 , 作以下假设 :
(1) Qk 稳定 , Vk 为零均值 、白噪声 , 但其方差 Rk 随时间分段变化 ;
第 6 期
卞鸿巍 , 等 :组合导航系统新息自适应卡尔曼滤波算法
100 1
于可见卫星数目 、多路径效应和仪器内部的测量噪 声等 多种因素 的影响 , GPS 的 测量噪声 会发生 变 化 , SKF 由于无法对 上述变化进行检测和调整 , 将 出现严重的估计偏差. 为此 , 人们提出了多种自适应 滤波算法 , 如 Sage-H usa 自适应算法或各种带衰减 因子或记忆因子的卡尔曼滤波器技术等[ 2] . 目前 , 人 们对自适应卡尔曼滤波器的研究逐渐集中在两个方 面 :基于新息自适应估计(IA E)的卡尔曼滤波器[ 3] 和多模型卡尔曼滤波器(MM A E)[ 4] . 其中 IA E 方法 进一步与模糊控制相结合 , 产生了多种基于模糊推 理的自适应卡尔曼滤 波技术[ 5 , 6] . 上述方法不 用增 加系统状态维数 , 在降低计算量的同时 , 可以获得较 好的精度. 但是文献中对模糊推理规则以及隶属度 函数确定等关键问题均未给出明确的方法.
收稿日期 :2005-04-07 作者简介 :卞鸿巍(1972-), 男 , 江苏泰兴人 , 副教授 , 主要从事惯性技术及组合导航技术研究. 金志华(联系人), 男 , 教授, 博士生导师 ,
电话(T el. ):021-62933783 ;E-mail :zh jin @sj tu. edu. cn.
The Innovat ion-Based Estimation Adapt ive Kalman Filter Algorithm for INS/ GPS Integrated Navigat ion System
B I AN H ong-w ei 1 , 2 , J I N Z hi-hua1 , W A N G J un-pu1 , T I AN Wei-f eng1 (1. Dept. of Inst rument Science and Eng . , Shanghai Jiao tong Univ. , Shanghai 200030 , Chi na ;
2. 1 组合系统
目前进行的船用 INS 与 GPS 姿态测量系统组 合导航研究中 , 采用 T rim ble MS860 DGPS 系统和
SBX-2 型差分 信标机 , 提 供 INS 修 正所必 须的 位
1 自适应卡尔曼滤波算法
1. 1 IAE 自适应卡尔曼滤波算法
对于离散线性系统模型 , 其状态方程和量测方
程如下 :
Xk =Υk, k- 1 Xk- 1 +Γk-1 Wk- 1
Zk =Hk Xk +Vk 其中 :Υk, k -1 为一步转移阵 ;Γk - 1 为系统噪声驱动阵 ;
Hk 为量测阵 ;Vk 为量测噪声序列 ;Wk 为系统激励 噪声序列 , 且 Wk 和 Vk 互不相关 并满足 :E[ Wk ] =
滤波增益 :
Kk*
=P
k
H
T k
C^-v
1 k
(9)
下面的数学推导将证明 C^v k 是 Cv k 的极大似然
最优估计.
1. 2 极大似然法数学证明 采用极大似然法对式(8)的结论给予证明.
记 r ={r 11 , r22 , … , rn n }, r ii 为 R 主对角线上的 噪声方差系数. 根据中心极限定律 , 采用高斯概率密
第 40 卷
式(14)第 1 项可以被忽略. 根据 r 的定义和 1. 1 节
的假设(2), r 为 R 主对角线元素 , 在有限记忆宽度
为 N 的新息估计窗口中 , 式(14)第 2 项可视为单位
阵 I. 故式(14)可以改写为
Cv k / rk =I
(1 5)
将式(15)代入式(13), 得
第 40 卷 第 6 期 2006 年 6 月
上 海交 通大 学学 报
JO U RN A L O F SH AN G HA I JIA O TO N G UN IV ERSIT Y
文章编号 :1006-2467(2006)06-1000-04
V ol. 40 N o. 6 Jun . 2006
本文针对 GPS 外观测误差变化问题 , 在传统卡 尔曼滤波器的基础上 , 提出了一种新的在线新息自
适应调整卡尔曼增益阵方法. 理想状态下稳态滤波 器新息应为零均值白噪声序列 , 一旦滤波器工作失 常 , 新息序列统计特性将变复杂 , 故新息可用来作为
判断调整滤波器增益的依据. 该方法通过实际新息 的测量计算直接修正卡尔曼滤波器增益 , 提高了在 GP S 测量发生较大变化时卡尔曼滤波器的 精度和 鲁棒性.
由式(7)Cv k 定义 , 得到 Cv k 对于 r 的偏导为
Cv k rk
=Hk
P
k
rk
H
T k
+
Rk rk
(14)
由式(2)可知
,
P
k
=ΥPk ΥT
+Qk -1 . 根据 1. 1 节
的假设(1), 系统噪声稳定并与测量噪声无关 , 所以
10 02
上 海 交 通 大 学 学 报
组合导航系统新息自适应卡尔曼滤波算法
卞鸿巍1, 2 , 金志华1 , 王俊璞1 , 田蔚风1
(1. 上海交通大学 仪器科学与工程系 , 上海 200030 ;2. 海军工程大学 电气工程系 , 武汉 430033)
摘 要 :全球定位系统(GPS)量测噪声的不稳定变化将造成惯性导航系统(INS)/GP S 舰用组合 系统卡尔曼滤波器性能下降 , 在对自适应卡尔曼滤波器分析的基础上 , 提出了一种新的基于新息估 计的自适应卡尔曼滤波算法. 该算法通过计算新息方差强度的极大似然估计最优估计 , 将新息方差 计算直接引入卡尔曼滤波器的增益计算. 仿真结果表明 , 本文方法较标准卡尔曼滤波器可以提高系 统精度和抗干扰能力. 关键词 :惯性导航系统 ;全球定位系统 ;组合导航系统 ;自适应卡尔曼滤波 ;新息自适应估计 中图分类号 :U 666. 1 文献标识码 :A
将惯性导航系统(INS)和全球定位系统(GP S)
结合起来构成更高精度的组合导航系统备受人们关 注. 在以往的文献中 , 绝大部分采取 GP S 的位置或 速度信息作为 IN S 的外部修正信息[ 1] . 本文采取可
以提供位置 、速度 、航向和纵摇姿态的高精度 G PS 姿态测量系统与 INS 进行组合 , 以期获得更加优越 的舰船组合导航性能. 标准卡尔曼滤波器(SKF)需 精确了解外部测量噪声的统计规律 , 但在实际中 , 由
(12)取 r 的偏导 , 根据矩阵计算公式[ 4] :
ln
|Cv j r
|=|C1v j
|
|Cv j r
|=tr
C- 1 vj
Cv j r
ln
C -1 vj
r
=-
C -1 vj
Cv r
j
C
v
1 j
得
∑k
j =j0
பைடு நூலகம்
tr
C- 1 vj
Cv j r
-
vTj
C
v
1 j
Cv r
j
C
v
1j v j
=0 (13)