无人系统导航定位技术---卡尔曼滤波与组合导航技术

被估计向量
Zi Hi X Vi
量测向量
量测矩阵
随机量测噪声
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
最小二乘估计
Z1 H1X V1 Z2 H2 X V2 Zr Hr X Vr
mn
m1
m1
Z HX V
n1
指标函数：J( Xˆ ) (Z HXˆ )T(Z HXˆ ) min
J
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
最小方差估计 原理：被估计量估计误差方差最小。
设 X 为随机向量， Z 为 X 的量测向量，即 Z Z(X ) V，
求 X 的估计 Xˆ 就是根据 Z 解算出 X ，显然 Xˆ 是 Ｚ 的函
数，由于V 是随机误差，所以 X无法从 Z的函数关系式中
最小方差估计 根据题意，量测方程为：
Z HX V
Z Z1 Z2 Zm T H 1 1 1T
V V1 V2 Vm T
根据公式有：
Xˆ MV (Z ) mX
P CX CV
mCX mCX CV
直接求取，而必须按统计意义的最优标准求取。
Hale Waihona Puke Baidu
J (Xˆ )
定理
1
EX,Z {[ X
Xˆ (Z)]T[X
Xˆ (Z)]}
Xˆ ( Z ) Xˆ MV ( Z )
min
最小方差估计等于量测为某一具体实现条件下的条件均值：
Xˆ MV (Z) E[X / Z]
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
估计的概念
X (t)
待求系统状态
Z(t) hX (t)V (t) 解算 Xˆ (t)
X(t)的估计
测量得出 的数据
测量噪声
Xˆ (t) 是Z(t)的函数,若为线性函数,则 Xˆ (t) 称作X(t)的线性估计
1 最优估计与卡尔曼滤波 1.1 最优估计的基本概念
(Z Zˆ)T (Z Zˆ) min
则所得估计为最小二乘估计！
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
最小二乘估计
该方法由高斯(Karl Gauss)在1795年测定行星轨道而提 出的参数估计算法。该算法特点是简单，不必知道被估计 量及量测值相关的任何统计信息。
原理：误差平方和最小。
最小方差估计 定理 2
最小方差估计是 X 的无偏估计。
定理 3
E(Xˆ MV (Z)) E[X ]
若被估计向量Xn1 和量测向量 Zm1 都服从正态分布，且
E[X ] mX , E[Z] mZ
Cov[ X , Z] E[( X mX )(Z mZ )T ] CXZ
Var[ Z] E[(Z mZ )(Z mZ )T ] CZ 则 X 的最小方差估计为：
卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的， 对物体位置的观察序列（可能有偏差）预测出物体的位置的 坐标及速度。在很多工程应用(如雷达、计算机视觉)中都可 以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制 系统工程中的一个重要课题。当输入为带有高斯白噪声的信 号时，使期望输出和实际输出之间的均方根误差达到最小的 线性系统，这种滤波方法以它的发明者鲁道夫.E.卡尔曼的名 字命名为卡尔曼滤波。
C
1m
X
X
)
P (C 1 H TC1H )1
X
V
例：设 X 为服从正态分布的随机量，均值为 mX 方差为
CX ，对 X 用 m台仪器同时直接测量，测量误差都是服 从正态分布的随机变量，均值为零，方差为 CX ，求 X
的最小方差估计和估计的均方差。
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
无人系统导航定位技术
---卡尔曼滤波与组合导航技术
主要学习内容
• 最优估计与卡尔曼滤波 • 组合导航基本原理和方法
学习参考资料
1.秦永元.卡尔曼滤波与组合导航原理.西北工业大学出版社 2.付梦印等.Kalman滤波理论及其在导航系统中的应用 3.王志贤 编著.最优状态估计与系统辨识.西北工业大学出版社.
Xˆ MV (Z)
mX
C
XZ
C
1
Z
(
Z
mz )
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
最小方差估计 估计的均方误差为：
Var[ X Xˆ MV (Z)] P CX CXZCZ1CZX
对于线性关系：
Z HX V
E(V ) 0,Var(V ) CV , E(X ) mX ,Var[X ] CX X 和 V
互不相关，则: Xˆ MV (Z ) mX (CX H T CV )1(Z Hm X )
P CX CX H T (HC X H T CV )1 HC X
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
最小方差估计 还可写成：
Xˆ MV
(Z)
(C 1 X
H
TC 1H )1(H V
TC 1Z V
预测和平滑
设在[t0，t1] 时间段内量测为Z，待求状态为 X (t)
当t=t1时, Xˆ (t) 称为X(t)的估计； 当t>t1时, Xˆ (t) 称为X(t)的预测；
t0
t1
Z 计算
t
Xˆ (t)
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
达到
某一指标函数
最小
若以量测估计的偏差的平方和达到最小为指标
(Z HXˆ )T(Z HXˆ ) 0
X X Xˆ
Xˆ (H T H )1 H T Z
1 最优估计与卡尔曼滤波
1.1 最优估计的基本概念
最小二乘估计的特点：
优点：算法简单，不必知道量测误差的统计信息； 局限性：
（1）只能估计确定性的常值向量，无法估计随机向量的 时间过程；
（2）最优指标只保证了量测的估计均方误差之和最小， 而并未确保被估计量的估计误差达到最佳，所以估计精度 不高。
卡尔曼
鲁道夫·卡尔曼（Rudolf Emil Kalman），匈 牙利裔美国数学家，1930年出生于匈牙利首都 布达佩斯。 1953年于麻省理工学院获得电机工 程学士，翌年硕士学位。1957年于哥伦比亚大 学获得博士学位。1964年至1971年任职斯坦福 大学。1971年至1992年任佛罗里达大学数学系 统理论中心（Center for Mathematical System Theory）主任。1972起任瑞士苏黎世联 邦理工学院数学系统理论中心主任直至退休。 先居住于苏黎世和佛罗里达。2009年获美国国 家科学奖章。