总结卡尔曼滤波

一章

一、导航与制导的区别：

导航：测量载体的导航参数（俯仰角、滚转角、偏航角），位置、速度和加速度。 制导：根据预先规划好的航线，自动引导载体到达目的地的技术和方法。 二、导航技术：

1. 惯性导航：

 加速度计：加速度到速度到位置。（有误差积累，故组合）  角度测量陀螺：测量角度（定轴性和进动性）  惯性执行机构：力矩陀螺、惯性动量轮。

优点：自主性强、短时间内精度高、连续提供位置与速度与加速度和姿态信息缺点：误差随时间积累、价格昂贵。 2. 天文导航：

通过观测天体和不同的时刻的同一天体。

优点：完全自主，误差不积累、不仅可以得到位置信息还可以得到姿态信息。缺点：定位信息不够高，输出信息不连续。 3. 卫星导航（无线电导航）

优点：精度高、误差无积累、全球全天候全天时。 缺点：输出不连续、不输出姿态信息。

三、惯导系统误差：

1. 确定性误差：惯性器件常值误差、

安装误差 标度因数误差

与加速度与关的误差

2. 随机误差：随机常数

一阶马尔可夫过程、 白噪声

加速度计与陀螺漂移

Kalmen滤波就是对付随机误差。 Kalmen滤波的条件:线性系统

非高斯噪声 （白噪声） 四、Kalmen滤波对线性系统与非高斯噪声系统的处理

1.非线性高斯噪声：EKF UKF 2.非高斯线性：PF

3.非线性与非高斯：UPF

二章、SIN/GPS的组合导航

IMU：高精度惯性测量单元 一、捷联惯导系统 ：

1.IMU的误差标定与补偿 （被积函数）

标定原理：a、建立陀螺和加速度计误差方程（误差方程如何建立的？六个方程） b、转台试验求解误差方程中的系数 2.SINS的静基座初始对准 （积分初值） ？：积分运算必须知道初始值 粗对准：时间是标准。

精对准：精度是指标。

1

Kalman总结

3. 惯导系统的误差根源是：加速度计偏置与陀螺漂移，属随机误差（？标定误差，初

始对准？）消除随机误差用卡拉曼滤波。 4. 惯导系统误差方程

1. 计算地理坐标系法 Ψ角误差方程（？？？？？？？？ 方程的建立及化简推

导） r. rV V.Vfg. 

δV、r和Ψ分别为速度、位置和姿态矢量 Ω为地球自转角速度

ω为导航坐标系相对惯性坐标系的角速度矢量 ▽是加速度计常值偏值，ε是陀螺常值漂移 f是比力，△g是重力矢量计算误差，

2. 真实地理坐标系法

EEL NNcosL DDsinL NNcosLsinLL

EEL DDsinLcosLLΨ角误差方程：

VEtanL

NEsinLVNDN ENsinLVRREVE RtanLDcosLEVRE

DVNNREcosLRD在静基座条件下，最终可得： E

NVRsinLLEsinLN EVNRNsinLDcosLE

DVERtanLcosLLEcosLD静基座条件下的误差方程：

L1RVN VE RsecL 二、kalman方程的建立（作业）

1.建立系统方程

2

Kalman总结

2.建立量测方程（那个可以观测？） 3.离散kalman滤波方程 三、静基坐对准

提高静基座对准的精度和速度的方法： 四、GPS动态滤波的数学方程

什么意思？ 五、最小二乘法

最优估计（只要使函数值最小即可） 最小方差估计（方差最小） 最小二乘估计：

三章kalman滤波原理（课本二五章）

Kalman方程就是递推线性最小方差估计。 Kalman方程（五个）各个的意义。 一、离散卡尔曼滤波方程

Z(t)H(t)X(t)V(t)状态一步预测：

Xk/k1k,k1*Xk1

状态估计：

XkXk/k1K*(ZkHk*Xk/k1)

滤波增益：

TTKkPk/k1*Hk*(Hk*Pk/k1*HkRk)1

一步预测均方误差：

Pk/k1k,k1*Pk1*Tk,k1Qk1

估计均方误差：

Pk(IKk*Hk)*Pk/k1

初值的确定：

在滤波开始时必须有初值才能进行，选择的初值具有无偏性。（如何保证无偏性？）这样才能保证据方差阵最小。 二、连续kalman滤波方程：

连续系统的状态方程与量测方程：

(t)F(t)X(t)G(t)W(t) X

Z(t)H(t)X(t)V(t)

连续系统的滤波方程：

K(t)P(t)HT(t)R1(t)

ˆ(t)F(t)Xˆ(t)K(t)[Z(t)H(t)Xˆ(t)]X(t)P(t)FT(t)F(t)P(t)P(t)HT(t)R1(t)H(t)P(t)G(t)Q(t)GT(t) PR-1(t)为是量测噪声的方差强度阵 R(t)的逆阵

3

Kalman总结

Q(t)是系统噪声的方差强度阵 三、离散—连续系统的kalman滤波方程

实际估计状态的系统是连续的，而量测试时间间隔的，离散的。 系统的状态方程与量测方程：

(t)F(t)X(t)G(t)W(t) ZHXV Xkkkk 连续的系统可以采用微分的数值解法，也可以将系统离散化后用离散的方法求解。 连续系统离散化的实质：

 将连续系统的系统矩阵计算出离散系统的转换矩阵。

 将连续系统的噪声方差强度矩阵计算出离散系统的噪声方差矩阵。 系统矩阵的离散公式：？？/

系统的噪声方差强度矩阵的离散公式： ？？/

四、kalman在组合导航中的应用： 估计方法：

直接估计法：直接估计系统的状态量。系统的状态量出来以后经kalman滤波输出 间接估计法：估计两个系统的状态量的误差，然后补偿于状态量。

 输出校正：  反馈校正： 脉冲校正 速率校正 补偿校正

 输出校正与反馈校正的总结：

A、输出校正只校正系统的输量，而反馈校正校正系统的内部状态量。

B、输出校正的滤波器所估计的状态量是未经校正的导航参数误差，而反馈校正滤

波器所估计的状态量是经校正的导航参数误差

C、前者数值大，后者数值小，而系统是一阶线性系统，数值约最小近似的准确性

越高。所以反馈校正真实的反映系统的误差状态动态过程。

五、非线性系统的kalman滤波：

1、非线性最优估计问题的随机非线性系统的数学模型：

(t)f[X(t),t]G(t)W(t) X

Z(t)h[X(t),t]V(t) 或 Xkf[Xk1,k1]k1Wk1 Zkh[Xk,k]Vk

非线性系统的解决方法是线性化。非线性方程的理论解一定存在，而且这个理论解与实际解之间的差能够用一个微分方程表示，线性干扰方程或小偏差方程或摄动方程。

线性化的方法：

 按标称状态的线性化的kalman滤波方程 1）围绕标称状态的线性化

4

Kalman总结

标称状态：简单的说就是理论解。确切的说是干扰噪声为0时的解。

当：非线性系统的真轨迹运动（实际的）与标称轨迹运动（理论解）的偏差足够小的话，可以对标称状态（理论解）运用泰勒展开式。

2）离散型线性化kalman滤波方程 A、先离散化后线性化的方法： 离散化： Xkk,k1Xk1Ik1Wk1 ZkHkXkVk

注：非线性系统离散化麻烦！ 线性化：

Xk,K1k,k1Xk1 . . . B、先线性化后离散化的方法： 线性化： (n)

(n)

离散化：

nn

kk1k1k1k,k1

n

kkkk

比较先离散后线性化的话 方便。

 按最优状态估计线性化的kalman滤波方程 【先线性化后离散化，会更好】

对连续非线性系统用kalman滤波，即广义kalman滤波。 滤波方程的推到：

对连续系统线性化： X(t)F(t)X(t)G(t)W(t) Z(t)H(t)X(t)V(t) 对线性化后的系统离散化：(干扰方程) Xkk,k1Xk1Wk1 ZkHkXkVk  对系统矩阵与量测矩阵的求解： T为小量时：

X(t)F(t)X(t)(t)X(t)V(t)Z(t)HXIWXZHXV 5

Kalman总结

f1X(tk),tk1X(t)

1k1 k,k1IF(tk1)TIT  X(tk1)Xˆ k1 h1[X(tk),tk] Hh[X(t),t..kk]X1(tk)k X(t/X(tk)XˆnkXˆk/k1...k)

...

X(tk)Xˆk/k1按照线性kalman滤波的基本方程推到偏差的kalman滤波方程。

Xˆk/k1k,k1Xk1XkXˆk/k1Kk[ZkHkXˆk/k1]KPTT1kk/k1Hk[HkPk/k1HkRk]Pk/k1k,k1Pk1Tk,k1Qk1P(IKPTTkkHk)k/k1(IKkHk)KkRkKkZkZkh[Xˆnk,k]Zkh[Xˆnk/k1,k]

离散型线性广义卡尔曼滤波方程：

XˆXˆn k/k1kXˆk1f[Xˆk1,tk1]

XˆXˆXˆ kk/k1k

XˆK{Zh[Xˆ,k]}

kkkk/k1

KPHT[HPHTR]1

kk/k1kkk/k1kk

PPTQ

k/k1k,k1k1k,k1k1 P(IKH)P(IKH)Tkkkk/k1kkKkRTkKk

6

Kalman总结

注：初值的设定。

六、kalman滤波与系统可观测系性分析： 定量分析系统的可观测性？

四章、最优滤波在惯性/天文/卫星组合导航系统中的应用

一、基于集中滤波的SINS/CNS/GPS 组合导航方法

1、系统误差模型：

 加速度误差方程  速度误差方程  平台误差方程

2、SINS/CNS/GPS的集中滤波器设计

1）系统方程

2）量测方程

二、基于联邦滤波的SINS/CNS/GPS 组合导航方法

1、联邦滤波器结构

参考系统 子系统1 ˆ,1PXg1gˆ,PX111主滤波器 时间更新 ˆXgˆ,PXmmLF1 ˆ,1PXg2g子系统2 LF2 ˆ,1PXgNgˆ,PX222ˆ,PXggmPgˆ,PXNNN最优融合 子系统N LFN

2、联邦滤波器的结构与性能分析： 什么时信息：

 状态方程的信息量：

状态方程的信息量是与系统噪声的方差成反比的，因此可用Q-1表示状态方程信息量；  量测方程的信息量：

量测方程的信息可用量测噪声协方差的逆R-1表示 什么是信息分配：

整个系统的信息量被分到N个子滤波器和main滤波器中，信息的总量不变，遵守信息守恒。

信息分配系数的不同分六类：重置???

一类：噪声信息全部由主系统承担，子系统不承担信息。(零化式重置) 优点 缺点

二类：子系统和主系统均分到1/(N+1)（有重置）

7

Kalman总结

优点 缺点

三类：信息全部由子系统平均承担，1/N. （有重置） 优点 缺点

四类：信息全部由子系统平均承担，1/N.（无重置） 优点 缺点

五类: 噪声信息全部由主系统承担，子系统不承担信息。（有重置） 优点 缺点

六类:主系统与系统1分别承担1/2。（有重置） 优点 缺点 结论：

利用融合后的全局状态和协方差反馈重置子滤波器，提高子滤波去器的精度，但主滤波器也容易受子滤波器的影响。

如果不将融合后的全局状态和协方差反馈重置子滤波器，就不会产生交叉污染，容错性能大大提高，为全局估计协方差的一倍。

3、联邦滤波器的设计

系统状态方程与量测方程的书写： 子滤波器1

 状态方程：

姿态误差方程 速度误差方程 位置误差方程

惯性器件的误差方程

 量测方程：SIN/GPS采用浅组合的方法，GPS 提供位置与速度信息，采用SIN/GPS

的位置与速度之差作为kalman滤波的量测信息。 量测方程： 子滤波器2同1

三、SINS/CNS/GPS半物理仿真系统： 物理仿真系统的基本流程：

五章、kalman滤波在组合导航初始对准中的应用

一、 惯导系统的静基坐的初始对准 1.1粗对准与精对准 粗对准——快

精对准——准（误差补偿） 古典：先水平后方位 现在：水平方位同时对准

在静对准过程中对陀螺仪测漂和定标，补偿陀螺漂移和定标系数，提高对准精度。 1.2静基座初始对准方案

8

Kalman总结

（1）采用KALMAN滤波进行初始对准，就是将平台误差角ΨN，ΨE，ΨD从随机误差和随

机干扰中估计出来，通过系统的校正使平台坐标系与导航坐标系对准； （2）同时，尽可能估计出陀螺漂移和加速度计偏置；

（3）时间不长，因此陀螺漂移和加速度计偏置可看作常值；

（4）根据分离定理，对随机系统和最优估计和最优控制可以分开单独考虑，故可用卡尔曼

滤波器对平台误差角及惯性仪表的误差进行单独研究。 1.3惯导系统的误差方程

两类: Φ角误差模型:真实地理坐标系

Ψ角误差模型：计算地理坐标系

描述惯导系统误差特性的微分方程可分为： 平动误差方程：速度误差 位置误差 姿态误差方程：Φ角误差模型

Ψ角误差模型

一般取速度误差和Ψ角误差模型 惯导系统的Ψ角误差方程： .vVfg .rrV .

δV、r和Ψ分别为速度、位置和姿态矢量 Ω为地球自转角速度

ω为导航坐标系相对惯性坐标系的角速度矢量 ▽是加速度计常值偏值，ε是陀螺常值漂移 f是比力，△g是重力矢量计算误差，

在东北地坐标系下，

（

cosLcosL  0

0LsinLsin把以上三式代入上面Ψ角误差方程：

静基座初始对准时，位置和垂直方向速度可准确知道  惯导系统的Ψ角误差方程可得。

sinLN0rN0rL100000r 0cosL010000rE0EsinLcosLrD0rDL0001000)sinL000(2L0fDfEVNNVNg/R0VEE)sinL)cosLVg/R0(20(2fD0fNE)cosL02g/RL(20fEfN0VDDVD00)sinL000000(LNNN00000()sinL0()cosLEEE0)cosL00000L(0D0DD 惯导系统的Φ角误差方程：

根据Ψ角与Φ角的关系，求解Φ角误差方程。

NNcosL

9

Kalman总结

EEL DDsinL

微分：

LE E

cosLsinLLNN

sinLcosLLD D惯导系统的Ψ角误差方程：

VV ENsinLEtanLDcosLERR

VVEtanLDNN NEsinLRRVV DNNEcosLEDRR

在静基座条件下

VEsinLLsinLEN：N RVNsinLcosL ENDER

VEtanLcosLLcosLED DR基座条件下速度误差方程： EVN2sinLVEEgN

基座条件下位置误差方程： 1LVN R VEsecL R最终可得，平台惯导系统的Φ角误差方程： 001/R0000LLsecL 000000R V0002sinL0g0VNN VV002sinL0g00EE

NsinL001/R0sinL0N  001/R0sinL0cosLEE 0tanL/R0cosL0DDcosL0

1.4卡尔曼滤波方程的建立

系统方程的建立和量测方程的建立： 系统方程：

2sinLVgVENNE

10

Kalman总结

量测方程：

1.5计算机仿真研究

ΨN和ΨE的估计精度由▽E和▽N决定 ENEN gg

ΨD的估计精度由εE决定

 DEEtgLENgN

1.6静基座初始对准的可观测度分析

1. 根据线性定常系统可观测性判定准则： 2. 利用奇异值分解来求秩，可仔细分析！

1.7提高静基座初始对准精度与速度的方法 1. 提高对准精度——>提高系统可观测度 2. 提高对准速度——>快速对准方法

二、惯导系统的动基坐的初始对准

2.1捷联惯导动基座对准的可观测度分析方法研究(基座的运动对观测有影响)

 惯导动基座对准时为线性时变系统，可观测性分析十分复杂

 PWCS可观测性分析理论与方法可确定状态是否可观测，无法确定状态的可观测程

度

 状态的可观测程度才是真正反映卡尔曼滤波中状态变量估计的速度和精度

 基于特征值和特征向量的可观测度分析方法，可以确定状态变量的可观测程度，但

是必须在滤波解算之后，计算量巨大！特征值越小,对应的状态变量可观测度越高  基于奇异值分解的可观测度分析方法，直接利用可观测矩阵实现系统可观测度分析 可观测矩阵的奇异值越大，其对应的状态变量可观测度越大！

2.2各种运动对捷联惯导系统状态变量可观测度的影响 （基座的运动【哪几种？】对观测有影响）

 静基座对准εx，▽x ， ▽y完全不可观测，εz可观测度很小0.0005  三轴摇摆提高了两个水平加计的可观测度

 匀速运动各个状态变量的可观测度与静基座基本相同

 线加速运动可大大提高方位失准角的可观测度，不能提高▽x ， ▽y计偏置的可观

测度

 三轴摇摆与线加速运动组合各个状态的可观测度都得到提高  航向变化与线加速运动组合各个状态的可观测度最高

11

Kalman总结

2.3捷联惯导动基座对准最优机动方法研究（基座的运动【哪种影响最大？】对观测有影响）

 S机动为动基座对准的最优机动方式  全部状态变量都能得到较好的估计

 估计精度还与载体S机动的机动程度有关 2.4捷联惯导动基座对准的H∞滤波方法

1. 新的算法具有良好的稳定性和跟踪性； 2. 克服了动基座对准中的随机干扰问题 三、惯导系统的传递对准

3.1传递对准技术概述 1. 什么是传递对准

传递对准是指载体航行时，载体上需要对准的子惯导利用已对准好的主惯导的信息进行初始对准的一种方法。

传递对准是一种动基座对准方法，它除了具有动机座对准的一般规律外， 还具有其 有的特点及性质 2. 传递对准的框图

载体运动状态

主惯导 杆臂效应 弹性变形 弹性振动 信息差 子惯导 卡尔曼 滤波 校正参数

传递对准的基本原理图

3.2不同匹配方案研究

（不明白匹配是什么？？？？？？？） 匹配的数学模型：

位置匹配 速度匹配 姿态角 角速度 组合方案 总结：

12

Kalman总结

位置匹配方案精度较低，且收敛速度慢，受机动方式影响不大；

速度匹配精度受速度变化影响，采用机动方式可提高对准速度和精度；

姿态角方案精度不高，速度不快，不能通过机动方式提高对准的速度和精度 角速度在S机动时对准精度很高，速度也很快，但这里没考虑载体弹性振动；

几种组合匹配方案的对准精度高，速度快，且对准的精度和速度随载体机动程度 增大而提高。

3.3运动方式对传递对准的影响（失准角是？？？？？？？？） 几种机动方式：

 直线飞行 仿真结论:

 两个水平失准角收敛很快，在20秒内达到稳态

 天向失准角在加速度变化时，可观测度提高，在加速度变化20秒后，也达到了稳

态

 三个水平失准角的协方差都在0.01ο左右

 水平加速飞行时，X向加速度计估计不准，其余尚可  水平盘旋 仿真结论:

 30秒后失准角进入稳态。

 其中，东向失准角收敛最快，只需20秒即可。  三个水平失准角的协方差都在0.01ο左右

 水平盘旋时，X、Y向陀螺、X、Z向加速度计估值均不准确。不宜采用此种对准方

法

 水平S机动 仿真结论:

 水平S机动时，全部状态都可以得到较好的估计  估计的精度与机动程度有关  抖翼机动 仿真条件:

 飞行轨迹：0-300秒，水平北向飞行，速度200米/秒；机翼做振幅30 o，周期2

秒的正弦晃动。其余飞行参数同东向水平飞行起始经、纬度均为45°  陀螺常值漂移:0.5°/h；  加速度计常值偏置:100ug；

 三个初始失准角分别为[1° 1° 1°]  观测速度噪声方差：0.01米/秒

（以下部分课件上讲的很清晰） 3.4机翼弹性振动对传递对准的影响

弹性振动引起的误差：圆锥误差、划船误差、尺寸误差。 尺寸效应误差： 补偿误差： 仿真结论：

振动对天向失准角的估计精度影响不大。

东向和北向的失准角估计精度分别下降了2.5度和0.5度。 加速度计和陀螺仪的零位误差估计值完全失真。

    

13

Kalman总结

补偿弹性振动后，对准速度和精度都有明显的提高。 3.5滤波周期对传递对准的影响

1. 滤波周期对kalman滤波的影响

 滤波周期对kalman滤波收敛速度有一定的影响

 缩短滤波周期可以有限度的缩短kalman滤波的收敛时间，但计算量急剧，实时

性差。

 随着滤波周期的不断减小，滤波周期对卡拉曼滤波收敛速度的影响越来越小。 2. 滤波周期对传递对准的影响 仿真结论：

 对于同一飞行轨迹，当滤波周期分别是0.01秒、0.1秒、1秒时，滤波收敛时间分别是

10秒、12秒、16秒。

 通过缩短滤波周期的却可以缩短收敛时间，但效果有限。  对准时间的缩短速度随着滤波周期的缩短而变慢。  大的滤波周期对惯性器件的误差估计精度损失较大，而对失准角的误差估计精度的损失

不明显。

对滤波周期设定的建议：

1) 仿真试验为目的的：

所选的滤波周期为0.1秒。 2) 实际应用为目的：

滤波周期一般选为0.1秒，以缩短对准时间和对准精度。

3.6载体弹性变形对传递对准的影响

载体的弹性变形对传递对准的影响较大。直接导致对准精度的下降。尤其对舰载武器的传递精度。

载体弹性变形的解决方案：将载体的弹性变形规律用系统状态方程描述. 状态方程 量测方程： 速度+姿态 速度+角速度 仿真结论：

 采用状态方程描述载体弹性振动，有效的提高对准精度。  速度+角速度的方案收敛速度更快，精度更高。  使用与主、子惯导都是捷联惯导的情况。

七章、机载SAR运动补偿用SIN/GPS组合导航技术

一、系统总体方案与误差分析 系统总体设计方案

14

Kalman总结

二、IMU的精确标定

三、SIN/GPS的先进滤波方法

四、空中机动对准与飞行试验

15