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

(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)
收稿日期 :2005-04-07 作者简介 :卞鸿巍(1972-), 男 , 江苏泰兴人 , 副教授 , 主要从事惯性技术及组合导航技术研究. 金志华(联系人), 男 , 教授, 博士生导师 ,
电话(T el. ):021-62933783 ;E-mail :zh jin @sj tu. edu. cn.
(2) 各段噪声变化强度未知 , 之间互不相关 ; (3) 在有限记忆长度为 N 的滑动采样区间 , 新
息采样具备遍历性.
记 C^v k 为 C v k 在长度为 N 的滑动采样的最优估 计值 , 则有
k
∑ ^Cv k
=1 N
vjv
Hale Waihona Puke T jj =j0(8)
将式(8)代入式(3), 即得改进的 IAE 自适应卡尔曼
将惯性导航系统(INS)和全球定位系统(GP S)
结合起来构成更高精度的组合导航系统备受人们关 注. 在以往的文献中 , 绝大部分采取 GP S 的位置或 速度信息作为 IN S 的外部修正信息[ 1] . 本文采取可
以提供位置 、速度 、航向和纵摇姿态的高精度 G PS 姿态测量系统与 INS 进行组合 , 以期获得更加优越 的舰船组合导航性能. 标准卡尔曼滤波器(SKF)需 精确了解外部测量噪声的统计规律 , 但在实际中 , 由
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. 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)
度函数表示在 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
第 6 期
卞鸿巍 , 等 :组合导航系统新息自适应卡尔曼滤波算法
100 1
于可见卫星数目 、多路径效应和仪器内部的测量噪 声等 多种因素 的影响 , GPS 的 测量噪声 会发生 变 化 , SKF 由于无法对 上述变化进行检测和调整 , 将 出现严重的估计偏差. 为此 , 人们提出了多种自适应 滤波算法 , 如 Sage-H usa 自适应算法或各种带衰减 因子或记忆因子的卡尔曼滤波器技术等[ 2] . 目前 , 人 们对自适应卡尔曼滤波器的研究逐渐集中在两个方 面 :基于新息自适应估计(IA E)的卡尔曼滤波器[ 3] 和多模型卡尔曼滤波器(MM A E)[ 4] . 其中 IA E 方法 进一步与模糊控制相结合 , 产生了多种基于模糊推 理的自适应卡尔曼滤 波技术[ 5 , 6] . 上述方法不 用增 加系统状态维数 , 在降低计算量的同时 , 可以获得较 好的精度. 但是文献中对模糊推理规则以及隶属度 函数确定等关键问题均未给出明确的方法.
-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 随时间分段变化 ;
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 ] =
本文针对 GPS 外观测误差变化问题 , 在传统卡 尔曼滤波器的基础上 , 提出了一种新的在线新息自
适应调整卡尔曼增益阵方法. 理想状态下稳态滤波 器新息应为零均值白噪声序列 , 一旦滤波器工作失 常 , 新息序列统计特性将变复杂 , 故新息可用来作为
判断调整滤波器增益的依据. 该方法通过实际新息 的测量计算直接修正卡尔曼滤波器增益 , 提高了在 GP S 测量发生较大变化时卡尔曼滤波器的 精度和 鲁棒性.
2. 1 组合系统
目前进行的船用 INS 与 GPS 姿态测量系统组 合导航研究中 , 采用 T rim ble MS860 DGPS 系统和
SBX-2 型差分 信标机 , 提 供 INS 修 正所必 须的 位
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
滤波增益 :
Kk*
=P
k
H
T k
C^-v
1 k
(9)
下面的数学推导将证明 C^v k 是 Cv k 的极大似然