04第四部分+卡尔曼滤波在组合导航中的应用

0 V x Vy 0 0 V z 1 x y 0 0 z
0 0 f34 0 0 0 1 0 0 0 0 1
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
ω p q r
i ib T
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.2 惯性导航原理简介
– 在地球固联坐标系内
dr ve dt
e
v i ωie r
T
ωie 0 0 ie
dv e dv e e i ω ie v e f 2ω ie v e g l dt dt e e e e e b e e e e v f 2 ω v g C f 2 ω v g e ie e l b ie e l e Ce Ω b C
最优滤波
位置、姿态 姿态
• 闭环反馈修正
CNS
姿态
最优估计
陀螺漂移修正
北京航空航天大学宇航学院 王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
– 位姿观测
INS 位置、姿态
位置、姿态
CNS
最优估计
位置、姿态反馈修正
北京航空航天大学宇航学院
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
第九章在组合导航中的应用
• 要点
– 掌握建模方法 – 掌握卡尔曼滤波的应用方法
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
• 导航
b b eb b e b e ω eb ωib Ce ωie
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.2 惯性导航原理简介
– 在地理坐标系内
dr vn n v i (ω ie ω en ) r dt dv e dv e n i (ω ie ω en ) v e f (2ω ie ω en ) v e g l dt dt n n n n b n n n n e v f n (2ωie ω en ) ve g ln Cn b f (2ω ie ω en ) v e g l n Cn Ω b C
Kalman Filtering
§9.6 组合算法
GM x2 3 (1 3 2 ) r r GM x( y R0 ) 3 3 r r2 GM xz 3 3 2 r r g x f15 y f 26 g y GM z ( y R0 ) r3 r2 g x f16 z g y f 26 z 3
§9.2 惯性导航原理简介
• 坐标系
– – – – 惯性坐标系 地球固联坐标系 地理坐标系 载体坐标系
d 2r ai 2 i f g dt dr vi i dt
王可东
• 惯性方程
– 在惯性坐标系内
北京航空航天大学宇航学院
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
– 深组合（Ultra-tightly-coupled GPS/INS）
卫星星历 位置，速度 计算INS 伪距伪距率 天线 惯性辅助 GPS接收机相 关控制回路 INS 速度位置 估计输出
I I ,
G G ,
Kalman 滤波器
反馈校正
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
o
w
i
i
光轴
os
ys
第i颗恒星 CCD面阵
f
xs
透镜
u
sin i cos i pos cos cos i i sin i
北京航空航天大学宇航学院
b poi CibCs pos
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.4 卫星导航原理简介
• 定位
( x xs )2 ( y ys )2 ( z zs )2 ctr lx ( x x s ) l y ( y y s ) lz ( z z s ) v
北京航空航天大学宇航学院
b b nb b b n n ωb nb ω ib C n (ω ie ω en )

北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.3 天文导航原理简介
• 定姿
v
zs
pi ui , vi
pui ui , 0
§9.2 惯性导航原理简介
T
dr dr i e ωie r v e ωie r dt dt ωie 0 0 ie dv e dv e d 2r d (ωie r ) i i i ωie v e ωie (ωie r ) 2 i dt dt dt dt dv e i f g ωie v e ωie (ωie r ) f ωie v e g l dt g l g ωie (ωie r ) 0 r q i i ie f i ωie v vie gil Cibf b ωie vie gil Ωiib r 0 p q p 0 i Ci Ωi C b b ib
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
• GPS/INS
– 松散组合（Loosely-coupled GPS/INS）
反馈校正 速度位置 估计输出
INS
速度，位置
Kalman 滤波器
GPS
速度，位置
北京航空航天大学宇航学院
王可东
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
• CNS/INS
– INS与CNS独立工作，CNS用于修正INS
• 已知载体的准确位置，通过CNS修正姿态，并修正INS的输出 • 根据INS提供的地平信息，进行CNS定位，并修正INS的输出
码和载波 NCO控制 计算
接收机原始数 据的预测 卫星位置 速度计算
卫星星历 数据解调
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.6 组合算法
• CNS/INS
– 系统建模
• 运动模型 x T φ y φω ε z vx 0 sin tan ie Rh v ie sin x tan 0 Rh vy cos vx ie Rh Rh
Kalman Filtering
§9.3 天文导航原理简介
• 定位
– 星光角距
r TS s arccos s r
– 星光折射
k1ek h
2
h r Tu r Ts tan Re vh
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
INS
位置、姿态
位置、姿态
CNS
位置、姿态
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
– CNS与INS组合用于修正陀螺仪的漂移
• 开环修正
INS
位置、姿态 姿态
位置、姿态 陀螺漂移和姿 态修正
CNS
INS
姿态
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
– 紧耦合（Tightly-coupled GPS/INS）
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
g x f14 x g f15 x y g f16 x z g y f 24 x g y f 25 y
z g f34 z x g f35 z y
g z GM z2 f36 3 (1 3 2 ) z r r
( R0 y ) 2 r x 2 ( y R0 ) 2 z 2 GM 3 (1 3 ) 2 r r
北京航空航天大学宇航学院
ie cos
vx R h x vy y ε Rh z 0
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.6 组合算法
0 0 0 0 f14 f 24 f15 f 25 f35 0 0 0 f16 Vx V f 26 y f36 Vz 0 x 0 y 0 z