卡尔曼滤波与组合导航—第五讲
合集下载
卡尔曼滤波器 ppt课件
卡尔曼滤波器的应用
• 卡尔曼滤波器对于解决阿波罗计划的轨 道预测很有用，后来阿波罗飞船的导航 电脑使用了这种滤波器。
• 它的广泛应用已经超过30年，包括导航 ，控制，传感器数据融合甚至在军事方 面的雷达系统以及导弹追踪等等,尤其是 在自动或辅助导航系统。近年来更被应 用于计算机视觉领域，例如人脸识别， 运动物体跟踪等等。
卡尔曼滤波器的思想
• 基本思想：卡尔曼滤波器提供了一种有 效的以最小均方误差来估算系统状态计 算递归方法。若有一组强而合理的假设， 给出系统的历史测量值，则可以建立最 大化这些早前测量值的后验概率的系统 状态模型。并且无需存储很长的早前测 量历史，我们也可以最大化后验概率， 即重复更新系统状态模型，并只为下一 次更新保存模型。这样就大大地简化了 这个方法的计算机实现。
• 最常用的是最小二乘估计，其他如风险准则的 贝叶斯估计、最大似然估计、随机逼近等方法 也都有应用。不管是维纳滤波还是卡尔曼滤波， 这些方法都只适用于线性系统，而且需要对被 估计过程有充分的知识。对于非线性系统或对 动态系统特性不完全了解的复杂估计问题，还 需要深入研究。工程上可用一些近似计算方法 来处理，常见的有基于局部线性化思想的广义 卡尔曼滤波器、贝叶斯或极大后验估值器和可 以根据滤波过程的历史知识自动修改参数的自 适应滤波或预报技术等
卡尔曼滤波器
1
卡尔曼滤波器
精品资料
你怎么称呼老师？ 如果老师最后没有总结一节课的重点的难点，你
是否会认为老师的教学方法需要改进？ 你所经历的课堂，是讲座式还是讨论式？ 教师的教鞭 “不怕太阳晒，也不怕那风雨狂，只怕先生骂我
笨，没有学问无颜见爹娘 ……” “太阳当空照，花儿对我笑，小鸟说早早早……”
• 卡尔曼滤波的实质是由量测值重构系统 的状态向量。它以“预测—实测—修正” 的顺序递推，根据系统的量测值来消除 随机干扰，再现系统的状态，或根据系 统的量测值从被污染的系统中恢复系统 的本来面目。
卡尔曼滤波与H∞滤波在INS／GPS组合导航中的应用
０ 弓Ｉ
舌
式 中 ｘ（） 为状 态矩 阵
组 合 导航通 常 采用传 统 的卡尔 曼 （ ｌ ｎ 滤 Ｋａｍａ ）
波方法 将 各种传 感 器 的信息融 合在 一起 ， 使得 构成 组 合 系统 的各项 性 能 指标 均优 于 ２个 子 系 统 单独 工 作 时的性 能 。但 是 在 对 参数 不确 定 系 统 和 有 色 噪声情 况 下 ， ｌ ｎ滤波器 效果 难 以令人 满意口 ， Ｋａｍａ ］ 而近 年来 提 出的 Ｈ 滤波方 法对 不 确定 和 有色 噪声
Ｉ ／ Ｓ组合 导航 ， 何 确 定 ｙ值 以更 好 地 提 高 ＮＳ ＧＰ 如 精度 是下 一 步研究 的重 点 。
原 理 ［ ． 安 ： 北 工 业 大 学 出 版社 ，０ ７ Ｍ］ 西 西 ２０．
作者简 介
参 考 文献
波算 法 与 Ｈ。滤波 算 法 ， 过 ＶＳ ０ ８编 程 实 现算 。 通 ２０
法 。对 于滤波 初值 的选 取 ， 样 频率 为 １ ｏＨｚ下 采 ０ ， 列参 数 由经验 确定 ： 状态 Ｘ 的初 值 全部 取 零 ， 陀螺
２ 卡尔 曼 滤 波 与 Ｈ。 波 方 程 。 滤
将 上 述 Ｉ ／ Ｓ组 合 导 航 模 型 离 散 化 后 分 ＮＳ ＧＰ 别 建立标 准 卡尔 曼滤 波算 法与 Ｈ 滤 波算 法
具 有 较 强 的 鲁 棒 性 能 ， 满 足 人 们 对 性 能 的 要 能
ｘ（）一 ［
８ ｖ
８ ８ １ ｗ ｆ］ × ￣
Ｆ ￡为连 续系 统 的状 态 转移矩 阵 （）
ｏ ｏ ０ ｏ
Ｆ ＝ ＝
一
２
求［ 。研究 了 Ｉ ／ Ｓ线 性 系 统 的 滤波 问题 ， ２ ］ ＮＳ ＧＰ 分 别用 卡尔 曼滤 波和 Ｈ 滤 波解 的实 例仿 真 说 明 了所 提 出方法 的可行性 和正 确性 。
卡尔曼滤波与组合导航课程实验报告
五、源程序
clear;
clc;
%载入数据
IMU=load('C:\Users\Administrator\Desktop\卡尔曼\IMU.dat');
GPS=load('C:\Users\Administrator\Desktop\卡尔曼\GPS.dat');
%%%%%%%%%%定义常数
e=1/298.3;
else
kesai=kesai_1-pi;
end
end
if Cnb(3,3)==0
if Cnb(1,3)>0
gama=pi/2;
else
gama=-pi/2;
end
elseif Cnb(3,3)>0
gama=gama_1;
else
if Cnb(1,3)>0
gama=gama_1-pi;
else
gama=gama_1+pi;
end
end
%%%%%%%%%%%%存储惯导解算求的的速度、位置和姿态角
velocity(i,:) = [vx,vy,vz];
position(i,:) = [lat/pi*180,long/pi*180,h];
gama=1.78357*pi/180 ; %横滚角
kesai=305.34023*pi/180 ; %航向角
q=[cos(kesai/2)*cos(cita/2)*cos(gama/2)-sin(kesai/2)*sin(cita/2)*sin(gama/2);
cos(kesai/2)*sin(cita/2)*cos(gama/2)-sin(kesai/2)*cos(cita/2)*sin(gama/2);
clear;
clc;
%载入数据
IMU=load('C:\Users\Administrator\Desktop\卡尔曼\IMU.dat');
GPS=load('C:\Users\Administrator\Desktop\卡尔曼\GPS.dat');
%%%%%%%%%%定义常数
e=1/298.3;
else
kesai=kesai_1-pi;
end
end
if Cnb(3,3)==0
if Cnb(1,3)>0
gama=pi/2;
else
gama=-pi/2;
end
elseif Cnb(3,3)>0
gama=gama_1;
else
if Cnb(1,3)>0
gama=gama_1-pi;
else
gama=gama_1+pi;
end
end
%%%%%%%%%%%%存储惯导解算求的的速度、位置和姿态角
velocity(i,:) = [vx,vy,vz];
position(i,:) = [lat/pi*180,long/pi*180,h];
gama=1.78357*pi/180 ; %横滚角
kesai=305.34023*pi/180 ; %航向角
q=[cos(kesai/2)*cos(cita/2)*cos(gama/2)-sin(kesai/2)*sin(cita/2)*sin(gama/2);
cos(kesai/2)*sin(cita/2)*cos(gama/2)-sin(kesai/2)*cos(cita/2)*sin(gama/2);
导航6-2卡尔曼滤波与组合导航-第5讲
y z
δ y δ z εx εy εz ∇x ∇y ∇z
T
X 系统状态向量
X = φx φy φz δ Vx δ Vy δ Vz δ x δ y δ z εx εy εz ∇x ∇y ∇z
T
14 /70
基于集中滤波的SINS/CNS/GPS SINS/CNS/GPS组合导航 2.2 基于集中滤波的SINS/CNS/GPS组合导航
12 /70
0 0 f14
f15
0 0 f24 f25 0 0 f34 f35 0 0 1 0 0 1 0 0 0 0 0 0
基于集中滤波的SINS/CNS/GPS SINS/CNS/GPS组合导航 2.2 基于集中滤波的SINS/CNS/GPS组合导航
(3) 系统误差模型 平台失准角误差方程
& ϕx C11 C12 C13 εx + Wεx ϕ = C C C ε + Wε &y 22 23 y y 21 ϕz C31 C32 C33 εz + Wεz &
9 /70
2.1 集中滤波概述
(3) 集中式滤波器缺点
集中式滤波理论上可以给出误差估计的最优估计， 集中式滤波理论上可以给出误差估计的最优估计， 但存在着如下缺点： 但存在着如下缺点： 状态维数高，计算负担重，不利于滤波实时运行， (1) 状态维数高，计算负担重，不利于滤波实时运行， 状态维数高会带来“维数灾难” 状态维数高会带来“维数灾难”； 容错性能差，不利于故障诊断，信息污染问题。 (2) 容错性能差，不利于故障诊断，信息污染问题。
13 /70
基于集中滤波的SINS/CNS/GPS SINS/CNS/GPS组合导航 2.2 基于集中滤波的SINS/CNS/GPS组合导航
δ y δ z εx εy εz ∇x ∇y ∇z
T
X 系统状态向量
X = φx φy φz δ Vx δ Vy δ Vz δ x δ y δ z εx εy εz ∇x ∇y ∇z
T
14 /70
基于集中滤波的SINS/CNS/GPS SINS/CNS/GPS组合导航 2.2 基于集中滤波的SINS/CNS/GPS组合导航
12 /70
0 0 f14
f15
0 0 f24 f25 0 0 f34 f35 0 0 1 0 0 1 0 0 0 0 0 0
基于集中滤波的SINS/CNS/GPS SINS/CNS/GPS组合导航 2.2 基于集中滤波的SINS/CNS/GPS组合导航
(3) 系统误差模型 平台失准角误差方程
& ϕx C11 C12 C13 εx + Wεx ϕ = C C C ε + Wε &y 22 23 y y 21 ϕz C31 C32 C33 εz + Wεz &
9 /70
2.1 集中滤波概述
(3) 集中式滤波器缺点
集中式滤波理论上可以给出误差估计的最优估计， 集中式滤波理论上可以给出误差估计的最优估计， 但存在着如下缺点： 但存在着如下缺点： 状态维数高，计算负担重，不利于滤波实时运行， (1) 状态维数高，计算负担重，不利于滤波实时运行， 状态维数高会带来“维数灾难” 状态维数高会带来“维数灾难”； 容错性能差，不利于故障诊断，信息污染问题。 (2) 容错性能差，不利于故障诊断，信息污染问题。
13 /70
基于集中滤波的SINS/CNS/GPS SINS/CNS/GPS组合导航 2.2 基于集中滤波的SINS/CNS/GPS组合导航
卡尔曼滤波算法及其在组合导航中的应用综述
卡尔曼滤波算法及其在组合导航中的应用综述摘要:由于描述系统特性的数学模型和噪声的统计模型不准确,不能真实反映物理过程,使模型与获得的观测值不匹配从而会导致滤波器发散。
文章在描述组合导航基本特性和卡尔曼滤波原理的基础上提出了滤波发散的问题并提出了抑制发散的方法,最后介绍了卡尔曼滤波在组合导航中的应用。
关键词:卡尔曼滤波;组合导航;发散随着计算机技术的迅速发展,它有条件提供运算速度高、存贮量大的机载计算机,这为组合导航系统的发展创造了一个很好的技术条件,现代控制理论中最优估计理论的数据处理方法为组合导航系统提供了理论基础。
Kalman滤波是R.E.Kalman于1960年提出的从众多与被提取信号有关的观测量中通过算法估计出所需信号的一种滤波算法。
他把状态空间的概念引入到随机估计理论中,把信号过程视为白噪声作用下的一个线性系统的输出,用状态方程来描述这种输入-输出关系,估计过程中利用系统状态方程、观测方程、系统噪声和观测噪声的统计特性形成滤波算法。
1组合导航系统基本特性描述要描述一个实际系统,首先要对其进行建模,即建立系统的状态方程和测量方程。
对于组合导航系统,要进行滤波计算必须建立数学模型,此模型具有以下特点。
1.1非线性组合导航系统本质上是非线性系统,有时为了减少计算量及提高系统实时性,在某些假设条件下组合导航系统的非线性因素可以忽略,其可以用线性化的数学模型来近似描述。
但当假设条件不满足时,组合导航系统就必须采用能反映自身实际特性的非线性模型来描述。
所以说,非线性是组合导航系统本质的特性。
1.2模型不确定性组合导航系统处于实际运行环境当中时,受系统本身以及外部应用环境不确定性因素的影响,系统实际模型与建立的理论模型不能完全匹配,即组合导航系统具有模型不确定性。
造成系统模型不确定性的主要原因如下:①模型简化。
采用较少的状态变量来描述系统,忽略掉实际系统某些不重要的状态特征。
由此造成模型与实际不匹配。
卡尔曼滤波与组合导航课程报告
《卡尔曼滤波与组合导航》课程实验报告实验捷联惯导 /GPS 组合导航系统静态导航实验实验序号 3姓名陈星宇系院专业17班级 ZY11172 学 号 ZY1117212日期2012-5-15指导教师宫晓琳成绩一、实验目的① 掌握捷联惯导 /GPS 组合导航系统的构成和基本工作原理； ② 掌握采用卡尔曼滤波方法进行捷联惯导/GPS 组合的基本原理；③ 掌握捷联惯导 /GPS 组合导航系统静态性能；④了解捷联惯导 /GPS 组合导航静态时的系统状态可观测性；二、实验原理（ 1）系统方程 X FX GWXTE NUvEvNvULhx y z x y z其中， E 、 N 、 U 为数学平台失准角；v E 、 v N 、 v U 分别为载体的东向、北向和天向速度误差；L 、 、 h 分别为纬度误差、经度误差和高度误差；x 、 y 、 z、x、y、z 分别为陀螺随机常值漂移和加速度计随机常值零偏。
（下标E 、 N 、 U 分别代表东、北、天）系统的噪声转移矩阵G 为：C b n03 3G03 3C b n9 39 315 6系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为：wwwww wTzwxyxyz系统的状态转移矩阵F 组成内容为：F NF SC b n3 3 ，其中 F N 中非零元素为可由惯导误差模型获得。
F S03 3 C b n 。
F069FM03 3 03 39 6（ 2）量测方程量测变量 zV E V NV ULT，，V 、V 、V 、L 、HENU和 H 分别为捷联解算与 GPS 的东向速度、北向速度、天向速度、纬度、经度和高度之差；量测矩阵 H H V H P T03 6 diag R M H , (R N H )cos L,036 ，， H PV 3 3diag 1, 1, 1 0 3 9 ，v v V E v V N v V U v v T H v为量测噪声。
量测噪声方0L H差阵 R 根据GPS的位置、速度噪声水平选取。
卡尔曼滤波.ppt
头脸识别 图像分割 图像边缘检测
Temperature Problem - Ideal World
假设当前室内温度仅跟上一时刻有 关 温度计观测（摄氏－〉华氏） 根据连续的观测值来推算实际温度 变化
Temperature Problem - Real World
假设当前室内温度仅跟上一时刻有 关
为
先验误差和后验误差odel - Algorithm
递推公式
如果没有误差，可以认为 则包含全部误差的信息，称为新息 (innovation) K为修正矩阵，或称混合因子 (Blend factor)
Blend factor Matrix
修正矩阵的形式有多种，其中一种为：
R->0 => K = 1/H
Discrete KF
Flow Chart
任意给定初值均可，但P!=0
Experiment
目标：
用KF估计一个常数（电压）
约束：
数据本身有误差（电压不稳）
观测有误差（电压表不准）
Analysis – Matrix Assignment
通过一种算法排除可能的随机干扰提高检测精度的一种手段线性系统?线性系统fabfafb?数学方法处理?噪声信号输入尽可能少噪声输出usefor?机器人导航控制?传感器数据融合?雷达系统以及导弹追踪?计算机图像处理?头脸识别?图像分割?图像边缘检测temperatureproblemidealworld?假设当前室内温度仅跟上一时刻有关?温度计观测摄氏华氏?根据连续的观测值来推算实际温度变化temperatureproblemrealworld?假设当前室内温度仅跟上一时刻有关?但变化中可能有噪声温度计观测摄氏?温度计观测摄氏华氏华氏?读数会有误差?两种噪声相互无关?根据连续的观测值来推算实际温度变化kalmanfilteringfirstsight?kf是根据上一状态的估计值和当前状态的观测值推出当前状态的估计值的滤波方法?stfst1ot?它是用状态方程和递推方法进行估计的因而卡尔曼滤波对信号的平稳性和时不变性不做要求?维纳滤波
卡尔曼滤波方法PPT课件
17
第17页/共28页
联邦滤波器算法
• 信息分配
在进入下一次递推之前，需将主滤波器中的信息 （状态、方差）在各子滤波器中按如下规则进行分配：
N
Xˆ i Xˆ g ,
Pii
P 1
ig
,
Q1
Qi1 Qm1
i 1
其中，Qi m1Q , i , i 1,, N, m 为信息分配系数，m 为
主滤波器的信息分配系数，满足守恒原则
方差估值 Pk k [I Kk Hk ]Pk k1
6
第6页/共28页
3.5 卡尔曼滤波的结构图
上述递推公式，称为卡尔曼滤波器。实际上，卡尔曼 滤波器也是一个系统，其结构框图如下：
Zk + -
+
Kk
+
Z k|k 1
当前估计值
Xˆ k
延时 一步
Hk
k ,k 1
一步预测
上一步估计值
Xˆ k|k 1
第27页/共28页
感谢您的观看！
28
第28页/共28页
Yi f ( i )
24
第24页/共28页
Unscented卡尔曼滤波(续) 变换样本点Yi 即可近似表示 y 的分布。下面利用 Yi 来计算 y 的均值和方差。
3. 计算 y 的均值和方差
p
y Wi(m)Yi
i0
p
Py Wi(c) (Yi y)(Yi y)T i0
其中，
Wi(m)
Wi(c)
得预测测量估计偏差： Z~k|k1 Zk Zˆk|k1 Zk Hk Xˆ k|k1
利用此偏差修正预测估计：
Xˆ k|k Xˆ k k1 Kk [Zk Hk Xˆ k k1]
第17页/共28页
联邦滤波器算法
• 信息分配
在进入下一次递推之前，需将主滤波器中的信息 （状态、方差）在各子滤波器中按如下规则进行分配：
N
Xˆ i Xˆ g ,
Pii
P 1
ig
,
Q1
Qi1 Qm1
i 1
其中，Qi m1Q , i , i 1,, N, m 为信息分配系数，m 为
主滤波器的信息分配系数，满足守恒原则
方差估值 Pk k [I Kk Hk ]Pk k1
6
第6页/共28页
3.5 卡尔曼滤波的结构图
上述递推公式，称为卡尔曼滤波器。实际上，卡尔曼 滤波器也是一个系统，其结构框图如下：
Zk + -
+
Kk
+
Z k|k 1
当前估计值
Xˆ k
延时 一步
Hk
k ,k 1
一步预测
上一步估计值
Xˆ k|k 1
第27页/共28页
感谢您的观看！
28
第28页/共28页
Yi f ( i )
24
第24页/共28页
Unscented卡尔曼滤波(续) 变换样本点Yi 即可近似表示 y 的分布。下面利用 Yi 来计算 y 的均值和方差。
3. 计算 y 的均值和方差
p
y Wi(m)Yi
i0
p
Py Wi(c) (Yi y)(Yi y)T i0
其中，
Wi(m)
Wi(c)
得预测测量估计偏差： Z~k|k1 Zk Zˆk|k1 Zk Hk Xˆ k|k1
利用此偏差修正预测估计：
Xˆ k|k Xˆ k k1 Kk [Zk Hk Xˆ k k1]
导航原理-组合导航PPT课件
上式说明，组合导航系统的导航参数的误差就 是惯导系统导航参数误差估值的估计误差。
.
9
2、反馈校正
采用反馈校正的间接法估计是将导航参数误差 的估值反馈到各导航系统内，对误差状态进行 校正。反馈校正的滤波示意图如图6.5所示
.
10
输出校正和反馈校正的分析
从形式看，输出校正只是校正系统的输出量，而 反馈校正则校正系统内部状态，但可以证明，如 果滤波器是最优滤波器，则两种校正方式的结果 是一样的。然而，真正意义上的“最优滤波器” 工程上是不存在的。未校正系统导航参数的误差 会随时间而增大，因而输出校正方式下的滤波器 状态值会越来越大。这使得方程线性化等近似计 算误差不断增大，从而滤波效果变差。
.
5
间接法估计的状态都是误差状态，即滤波 方程中的状态矢量是导航参数误差状态和 其它误差状态的集合（用 正和反馈校正。
1、输出校正
以惯导系统和其它某一导航系统组合为
例，间接法的组合导航卡尔曼滤波器将惯
导系统和其它导航系统各自计算的某些导
组合导航
.
1
2. 最优综合导航系统
采用卡尔曼滤波器的组合方法
卡尔曼滤波是一种递推线性最小方差估计，它 用“状态”表征系统的各个物理量，而以“状 态方程”和“观测方程”描述系统的动力学特 性。它要求应用对象是线性系统，且已知系统 的某些先验知识，如系统噪声和测量噪声的统 计特性。综合导航系统基本满足这些条件，因 而适合采用卡尔曼滤波。
(ie
cos L
VE RN
N
、)
hU
E
VN RM
h N
N
式中角注E、N、U 代表东、北、天；
RM Re (1 2 f 3 f sin 2 L)
《卡尔曼滤波》课件
3
无迹卡尔曼滤波线性系统的 估计。
卡尔曼滤波的应用案例
飞行器姿态估计
卡尔曼滤波在航空领域中被广泛应用于飞行器姿态估计，用于提高飞行器的稳定性和导航准 确性。
目标跟踪
卡尔曼滤波可用于跟踪移动目标的位置和速度，常见于机器人导航和视频监控等领域。
3 卡尔曼滤波的应用领
域
卡尔曼滤波被广泛应用于 航空航天、机器人、金融 等领域，用于提高系统的 状态估计精度。
卡尔曼滤波的数学模型
状态空间模型
卡尔曼滤波使用状态 空间模型表示系统的 状态和观测值之间的 关系，包括状态方程 和测量方程。
测量方程
测量方程描述观测值 与系统状态之间的关 系，用于将观测值纳 入到状态估计中。
了解更多关于卡尔曼滤波的内容和应用，推荐文献、学术论文和在线课程等资源。
《卡尔曼滤波》PPT课件
卡尔曼滤波是一种优秀的状态估计方法，被广泛用于目标跟踪、姿态估计和 股票预测等领域。
介绍卡尔曼滤波
1 什么是卡尔曼滤波？
卡尔曼滤波是一种递归状 态估计算法，用于通过系 统模型和测量信息估计系 统状态。
2 卡尔曼滤波的基本原
理
卡尔曼滤波基于贝叶斯估 计理论，通过最小化估计 误差的均方差来优化状态 估计。
股票预测
卡尔曼滤波可以应用于股票市场，通过对历史数据进行分析和预测，提供股票价格的预测和 趋势分析。
卡尔曼滤波的优化算法
粒子滤波
粒子滤波是一种基于蒙特卡洛 方法的状态估计算法，适用于 非线性和非高斯系统，提供更 广泛的估计能力。
自适应滤波
自适应滤波是一种根据系统的 特点自动调整滤波参数的方法， 提供更好的适应性和鲁棒性。
非线性滤波
非线性滤波是对卡尔曼滤波算 法的改进，用于处理非线性系 统和测量模型，提供更准确的 状态估计。
卡尔曼滤波教学课件PPT
5.卡尔曼滤波控制系统结构图
由于系统的状态x是不确定的，卡尔曼滤波 器的任务就是在有随机干扰w和噪声v的情 ˆ ，它在 况下给出系统状态x的最优估算值 x 统计意义下最接近状态的真值x，从而实现 最优控制u( x ˆ )的目的。
状态方程：X(k)=AX(k-1)+BU(k)+W(k) 输出方程：y(k)=CX(k)+Z(k) 系统测量值：Z(k)=HX(k)+V(k) 在上述方程中，X(k)是k时刻的系统状态， U(k)是k时刻对系统的控制量。A和B是系 统参数，对于多模型系统，它们为矩阵。 Z(k)是k时刻的测量值，H是测量系统的参 数，对于多测量系统，H为矩阵。W(k)和 V(k)分别表示过程噪声和测量噪声。它们被 假设成高斯白噪声，它们的协方差分别是Q， R。
6.2
更新阶段
新息或测量余量：y(k)=Z(k)-H X(k|k-1) 新息协方差:S(k)=H P(k|k-1) H’ +R 卡尔曼增益(Kalman Gain): Kg(k)= P(k|k1) H’ / (H P(k|k-1) H’ + R) …… (3) 状态估计更新:收集现在状态的测量值，结 合预测值和测量值，可以得到现在状态的 最优化估算值。
6.卡尔曼滤波过程
卡尔曼滤波包括两个阶段：预测和更新。 在预测阶段，滤波器应用上一状态的估计 做出对当前状态的估计。在更新阶段，滤 波器利用在当前状态的观测值优化预测阶 段的预测值，以获的一个更精确的当前状 态的估计。
6.1预测阶段
状态估计： 根据系统的模型，可以基于系统的上一状态而预 测出现在的状态。 X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1) 式(1)中，A是作用在前一状态的状态转移模型(状 态转移矩阵),B是作用在控制向量上的控制输入模 型(输入输出矩阵), X(k|k-1)是利用上一状态预测 的结果，X(k-1|k-1)是上一状态最优的结果，U(k) 为现在状态的控制量，如果没有控制量，它可以 为0 。
卡尔曼滤波与组合导航原理课件
X k 1
Xk
Zk
根据k-1时主刻要以适前用X于（线k）性也动可态以系说统是！综合利用k
所有一的次量仅测处值理得一到时个刻量以测前量的所有量测值得到 的 计算量大大减小
1 最优估计与卡尔曼滤波
1.2 离散卡尔曼滤波
离散卡尔曼滤波数学描述
设离散化后的系统状态方程和量测方程分别为：
X
k
k ,k 1 X k 1 k 1Wk 1 Zk H k X k Vk
J
(Z HXˆ )T (Z HXˆ ) 0
X X Xˆ
Xˆ (H T H )1 H T Z
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
最小二乘估计的特点：
优点：算法简单，不必知道量测误差的统计信息； 局限性：
（1）只能估计确定性的常值向量，无法估计随机向量的 时间过程；
（2）最优指标只保证了量测的估计均方误差之和最小， 而并未确保被估计量的估计误差达到最佳，所以估计精度 不高。
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
贝叶斯估计
设X 出的对
为的被X 估估计计，量，ZX~
是 X
X
的量测量，Xˆ (Z) 是根据 Z 给 Xˆ (Z为) 估计误差，如果标量函
数 L(X~) L[X Xˆ (Z)]
具有性质
（1）当 X~2 X~1 时，L(X~2) L(X~1) 0 （2）当 X~ 0 时，L(X~) 0 （3）L(X~) L(X~)
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
最小方差估计 原理：被估计量估计误差方差最小。 设 X 为随机向量， Z 为 X 的量测向量，即 Z Z (X ) V，
求 X 的估计 Xˆ 就是根据 Z 解算出 X ，显然 Xˆ 是 Ｚ 的函
卡尔曼滤波与组合导航
/
k
1H
T k
Rk )1
P P Q k /k1
T k ,k 1 k 1 k ,k 1
T k 1 k 1 k 1
Pk
(I
Kk Hk )Pk / k1(I
Kk Hk )T
K
k
Rk
K
T k
或
Pk (I Kk H k )Pk / k1
2、离散卡尔曼滤波方程
T k ,k 1 k 1 k ,k 1
T k 1 k 1 k 1
Pk
(I
Kk Hk )Pk / k1(I
Kk Hk )T
K
k
Rk
K
T k
13
各滤波方程的物理意义：
（1）状态一步预测方程
Xˆ k1
Xˆ k / k 1
Xk-1的卡尔曼滤波估值 利用Xk-1计算得到的一步预测
E X~X~T E[X~ EX~][X~ EX~]T
因此，最小方差估计不但使估值 X (Z)的均方误差最小， 而且这种最小的均方误差就是估计的误差方差
5
2、线性最小方差估计
如果将估值
X
规定为量测矢量Z的线性函数，即
X AZ b
式中A和b分别是（n×m）阶和n维的矩阵和矢量。这 样的估计方法称为线性最小方差估计。
最小方差 估计
3
线性最小 方差估计
递推线性最小 方差估计
1、最小方差估计
最小方差估计的估计准则是估计的均方误差最小，即：
Z是m维随机 量测向量
E{[ X X (Z )][ X X (Z )]T } E{[ X (Z )][ X (Z )]T }
卡尔曼滤波与组合导航原理—初始对准
.
27
2.3 惯导系统的误差方程
静基座初始对准时，位置和垂直方向速度可准确知道 惯导系统的误差方程可简化为：
rN 0 siL n L
1
0
0
0
0
0 rN 0
rE
rV D N
sL iL nc0oLsc0oLs
g/R 0
0
0 0 0
1
0
(2)siL n
0
1 L
0
0
0
0
0
fD
0 rE 0
0 fE
rV D N
惯导系统的Ψ角误差方程：
惯导系统的误差模型可由下列3个基本方程表示：
V V f g
r rV
（2.3.1）
• δV、r和Ψ分别为速度、位置和姿态矢量
• Ω为地球自转角速度
• ω为导航坐标系相对惯性坐标系的角速度矢量
• ▽是加速度计常值偏值，ε是陀螺常值漂移
• f是比力，△g是重力矢量计算误差，
静基座条件下速度误差方程：
速度误差定义为计算速度与真实速度之差
V N 2 sL iV E n E g N
V E 2 sL iV n N N g E
静基座条件下位置误差方程：
（2.3.9）
L
1 R
VN
VE secL
R
.
32
2.3 惯导系统的误差方程
最终可得，平台惯导系统的Φ角误差方程： 不考虑δλ平台惯导系统的Φ角误差方程可简化为：
可以证明两种模型是等价的！
.
23
2.3 惯导系统的误差方程
描述惯导系统误差特性的微分方程可分为：
两种
平动误差方程 表示形式
变量取为位置误差 变量取为速度误差
第五讲：卡尔曼滤波
…
第五讲：卡尔曼滤波
11
二、Kalman滤波
20.01.2021
第五讲：卡尔曼滤波
12
2.1 卡尔曼滤波方程
目 录 1. 离散系统的数学描述
设离散化后的系统状态方程和量测方程分别为：
概述 标准 KF 扩展 KF
Xk
X k,k 1 k 1
k 1Wk 1
Zk Hk Xk Vk
Schmidt KF
自适应 KF 平滑算法
标准卡尔曼滤波的线性假设在标准的卡尔曼滤波中观测模型假设为线性z是x的线性函数但实际情况往往并非如此如gnss导航滤波器中观测模型是强非线性的在标准卡尔曼滤波中系统模型也被假设为线性的x的时间导数是x的线性函数问题
卡尔曼滤波算法及应用
第五讲：卡尔曼滤波
目录
一. 概述
二. 标准卡尔曼滤波
卡尔曼滤波方程
滤波器估值的
以模某型种一导般航系都是线主导性要航的部参分数即误是差
统输出导航参
的估值
数的误差为主
要状态
24
目 录 2. 开环卡尔曼滤波
概述 标准 KF
用导航参数误差的估值 Xˆ去校正系统输出的导航参数，得到综 合导航系统的导航参数估值 Xˆ
扩展 KF Schmidt KF 自适应 KF 平滑算法
惯性系统 其他导航系统
描述了观测向量与状态向量间的函数关系。
第五讲：卡尔曼滤波
9
目录 概述
标准 KF 扩展 KF Schmidt KF 自适应 KF
1组观测向量
是一组针对同一时刻的系统特性的测量值，例如观测量可以包括GNSS系 统的位置测量值，或者INS与GNSS位置结果的差值。
1个算法：
卡尔曼滤波算法 使用观测向量、观测模型和系统模型来获得状态向量的最优估计，分为系
第五讲：卡尔曼滤波
11
二、Kalman滤波
20.01.2021
第五讲：卡尔曼滤波
12
2.1 卡尔曼滤波方程
目 录 1. 离散系统的数学描述
设离散化后的系统状态方程和量测方程分别为：
概述 标准 KF 扩展 KF
Xk
X k,k 1 k 1
k 1Wk 1
Zk Hk Xk Vk
Schmidt KF
自适应 KF 平滑算法
标准卡尔曼滤波的线性假设在标准的卡尔曼滤波中观测模型假设为线性z是x的线性函数但实际情况往往并非如此如gnss导航滤波器中观测模型是强非线性的在标准卡尔曼滤波中系统模型也被假设为线性的x的时间导数是x的线性函数问题
卡尔曼滤波算法及应用
第五讲：卡尔曼滤波
目录
一. 概述
二. 标准卡尔曼滤波
卡尔曼滤波方程
滤波器估值的
以模某型种一导般航系都是线主导性要航的部参分数即误是差
统输出导航参
的估值
数的误差为主
要状态
24
目 录 2. 开环卡尔曼滤波
概述 标准 KF
用导航参数误差的估值 Xˆ去校正系统输出的导航参数，得到综 合导航系统的导航参数估值 Xˆ
扩展 KF Schmidt KF 自适应 KF 平滑算法
惯性系统 其他导航系统
描述了观测向量与状态向量间的函数关系。
第五讲：卡尔曼滤波
9
目录 概述
标准 KF 扩展 KF Schmidt KF 自适应 KF
1组观测向量
是一组针对同一时刻的系统特性的测量值，例如观测量可以包括GNSS系 统的位置测量值，或者INS与GNSS位置结果的差值。
1个算法：
卡尔曼滤波算法 使用观测向量、观测模型和系统模型来获得状态向量的最优估计，分为系
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
一、惯导系统初始对准概述
1.1 初始对准的必要性
基本解决
理论问题
惯 导 系 统 的 问 题
理论、方法、指导
工程技术问题
难度（实现）
一、惯导系统初始对准概述
1.1 初始对准的必要性（续）
惯导系统
姿态矩阵 计算
一、惯导系统初始对准概述
1.1 初始对准的必要性（续）
加速度计
积分运算必须知道
a
∫
∫
V
∫
可以分开单独考虑，故可用卡尔曼滤波器对平台误差
角及惯性仪表的误差进行单独研究。
二、惯导系统的静基座初始对准方法 2.3 惯导系统的误差方程
惯导系统误差源
仪表误差
安装误差
Байду номын сангаас
初始条件 误差
运动干扰 （有害）
其他误差
如地球曲率半径描述误差； 有害加速度补偿中忽略二阶小量
2.3 惯导系统的误差方程
Φ角误差模型和Ψ角误差模型 基础 初始对准 惯导误差方程
卡尔曼滤波与组合导航原理
Theory of Kalman Filter and Integrated Navigation
第五章 卡尔曼滤波在惯性导航初始对准中的应用
一、惯导系统初始对准概述
二、惯导系统的静基座初始对准
三、惯导系统的动基座对准
四、惯导系统的传递对准
参考坐标系
1、建立参考坐标系的意义 宇宙间的一切物体都是在不断地运动,但对单个物体是无 运动可言的,只有在相对的意义下才可以谈运动.一个物体在 空间的位置只能相对于另一个物体而确定,这样,后一个物体 就构成了描述前一个物体运动的参考系. 参考系通常采用直角坐标系来代表,称为参考坐标系或简 称参考系.在研究陀螺仪或运载体的运动时,同样需要有参考 坐标系才成. 陀螺仪最重要的功用之一就是用它在运载体上模拟地理 坐标系或惯性坐标系。 常用坐标系：地心惯性坐标系、地球坐标系、地理坐标系、 载体坐标系。
Z
P
初始值！
, ,
X
Y
1.2 初始对准的分类
粗对准 对准阶段 精对准
自主对准
对外信息 的需求 基座 运动状态
静基座
初始对准
非自主 对准
动基座
对准轴系 水平对准 方位对准
一、惯导系统初始对准概述
1.3 初始对准的要求
初 始 对 准 的 要 求 对准精度
快又准
对准时间
对准精度与对准时间相互制约，不同场合侧重点不同
四、惯导系统的传递对准
二、惯导系统的静基座初始对准方法
2.1 2.2 2.3 2.4 2.5 2.6 2.7 粗对准与精对准 静基座初始对准方案
惯导系统的误差方程
卡尔曼滤波方程的建立
计算机仿真研究
静基座初始对准的可观测度分析
提高静基座初始对准精度与速度的方法
二、惯导系统的静基座初始对准方法
2.1 粗对准与精对准
一、惯导系统初始对准概述
1.4 初始对准的发展趋势
自适应滤波 H∞滤波 初 始 对 准 的 发 展 新的滤波方法
神经网络
非线性滤波 预测滤波
可观测性分析和 可观测度研究
从根本提高对准 的精度和速度
第五章 卡尔曼滤波在惯性导航初始对准中的应用
一、惯导系统初始对准概述
二、惯导系统的静基座初始对准
三、惯导系统的动基座对准
（2.3.1）
• ρ是地理系相对地球转动速度矢量
2.3 惯导系统的误差方程
惯导系统的Ψ角误差方程：
V V f g
r r V
在北东地坐标系中，有：
（2.3.1）
cos L cos L 0 （2.3.4） 0 （2.3.2） （2.3.3） sin L sin L
Φ=Ψ
2.3 惯导系统的误差方程
惯导系统的Ψ角误差方程：
惯导系统的误差模型可由下列3个基本方程表示： V V f g
r r V
• δV、r和Ψ分别为速度、位置和姿态矢量 • Ω为地球自转角速度 • ω为导航坐标系相对惯性坐标系的角速度矢量 • ▽是加速度计常值偏值，ε是陀螺常值漂移 • f是比力，△g是重力矢量计算误差，
根据对准精度的要求，静基座对准过程分为： 粗对准
• 要求尽快地将平台调整到 通常在精对准过程中要进行陀 一个精度范围内， 螺的测漂和定标，进一步补偿陀螺 • 缩短对准时间是主要指标 漂移率和标定刻度系数，以提高对 准精度 在粗对准基础上进行， • • 对准精度是主要指标
精对准
• 平台，先水平（调平），后方位，使系统有较好的动态性能 • 捷联：精确建立姿态矩阵 — 水平和方位对准同时（现代）
参考坐标系
★载体坐标系（OXbYbZb） 与载体固连，其原点与载体的重心重合, Xb轴沿载 体纵轴方向, Yb轴沿载体横轴方向，Zb轴沿载体竖轴方向。
Zb
Yb
Xb
实现惯导要解决的几个问题
平台跟踪坐标系 平台跟踪什么样的坐标系是平台式惯导系统的首要问题 舒勒摆原理在惯导系统中的应用 普通地平液体摆做敏感元件受加速度影响较大，需用舒 勒摆原理 有害加速度的消除 消除由于地球自转、飞机飞行引起的牵连、哥氏、重力 加速度等 初始对准问题 惯导系统要正确而精确的工作，必须精确给定初始条件 捷联惯导解算问题 数学平台代替机电平台
误差角取为 Φ 误差角取为 Ψ
Ψ ：平台坐标系与计算地理坐标系之间的误差角 Φ：平台坐标系与真实地理坐标系之间的误差角 目前大多采用Ψ角误差模型和速度误差表达形式！
2.3 惯导系统的误差方程
Ψ角误差模型+速度误差表达形式的优点： （1）平动误差不会耦合到姿态误差方程中， 特别便于动基座对准问题的分析和研究。 （2）Φ动角可通过位置误差和Ψ角得到： Φ=Ψ+θ （3）静基座时，惯导所处地理位置可精确获得， 且对准时间较短，可忽略位置误差，此时：
陀螺漂移
随机误差 惯导系统 为随机系统
若采用状态反馈控制，就必须对状态进行估计！
卡
尔
曼
滤
波
器
二、惯导系统的静基座初始对准方法
2.2 静基座初始对准方案(续)
（1）采用KALMAN滤波进行初始对准，就是将平台误差角 ΨN，ΨE，ΨD从随机误差和随机干扰中估计出来， 通过系统的校正使平台坐标系与导航坐标系对准； （2）同时，尽可能估计出陀螺漂移和加速度计偏置； （3）时间不长，因此陀螺漂移和加速度计偏置可看作常值； （4）根据分离定理，对随机系统的最优估计和最优控制
1
参考坐标系
当运载体在地球上运动时,运载体相对地球的位置不断 改变;而地球上不同地点的地理坐标系,其相对地球坐标系 的角位置是不相同的.也就是说，运载体相对地球运动引起 地理坐标系相对地球坐标系转动.这时地理坐标系相对惯性 坐标系的转动角速度应包括两个部分：一是地理坐标系相 对地球坐标系的转动角速度:另一是地球坐标系相对惯性坐 标系的转动角速度. 地理坐标系的三根轴构成右手直角坐标系，可以按 “北、东、天”、“北、西、天”或“北、东、地”顺序 构成。
•扩充为系统状态变量
•不完全为白噪声
2.3 惯导系统的误差方程
最终可得惯导系统的Ψ角误差方程：
2 sin L 0 g 0 VN 0 0 g 0 0 VE 2 sin L N 0 0 0 sin L 0 0 sin L 0 cos L E 0 0 0 0 cos L 0 D 0 0 0 0 N 0 0 0 0 0 0 E 0 0 0 0 N 0 0 0 0 0 0 E D 0 0 0 0 0 1 0 0 0 0 VN 0 1 0 0 0 VE 0 0 1 0 0 N 0 0 0 1 0 E 0 0 0 0 1 D 0 0 0 0 0 N 0 0 0 0 0 E 0 0 0 0 0 N 0 0 0 0 0 E 0 0 0 0 0 D
赤道
X Xe
本初子午线
Ω*t
Ye
参考坐标系
★地理坐标系（ONEZ） 其原点与运载体的重心 重合,E轴沿当地纬线指东,N 轴沿当地子午线指北,Z轴沿 当地地垂线指天.其中E轴与 N轴构成的平面即为当地水 平面,N轴与Z轴构成的平面 即为当地子午面. 这种地理坐标系是跟随 运载体运动的,更确切地说 应称为动地理坐标系或当地 地理坐标系.
参考坐标系
Z
2、几个参考坐标系的定义 ★惯性坐标系 通常把使得牛顿力学定律成立的参考坐标系,称为惯性 坐标系，简称惯性系； 赤道 根据选取的坐标系原点不同，分为日心惯性坐标系和 地心惯性坐标系。 日心惯性坐标系：原点取在太阳的中心,三根轴指向确定的 X Y 恒星。 地心惯性坐标系（OXiYiZi）：原点取在地球的中心,Xi和 Yi轴位于赤道平面内并指向确定的恒星，Zi轴与地轴（地 球自转轴）重合。地心惯性坐标系不参与地球自转。 惯性空间：惯性坐标系三根轴所代表的空间。
参考坐标系
★地球坐标系（OXeYeZe） 与地球固连，原点取在地球 的中心,Xe和Ye轴位于赤道平面 内，分别指向本初子午线和东经 90°子午线，Ze轴与地轴重合。 地球坐标系参与地球自转， 它相对于惯性坐标系的转动角速 度就等于地球自转角速度。 地球相对惯性空间的转动， 可以用地球坐标系相对于惯性坐 标系的转动来表示。 Ze Ω
— 先水平后方位（经典）
二、惯导系统的静基座初始对准方法
2.2 静基座初始对准方案
静基座初始对准分为两大类：