卡尔曼滤波实验报告
2014 年 4 月
GPS静/动态滤波实验
一、实验要求
1、分别建立GPS静态及动态卡尔曼滤波模型，编写程序对静态和动态GPS数据进行Kalman滤波。
2、对比滤波前后导航轨迹图。
3、画出滤波过程中估计均方差(P 阵对角线元素开根号)的变化趋势。
4、思考：① 简述动态模型与静态模型的区别与联系；② R 阵、Q 阵，P0阵的选取对滤波精度及收敛速度有何影响，取值时应注意什么；③ 本滤波问题是否可以用最小二乘方法解决，如果可以，请阐述最小二乘方法与Kalman 滤波方法的优劣对比。
二、实验原理
2.1 GPS 静态滤波
(deg)
度(m)
(1)
所以离散化的状态模型为：
(2)
可以表示为：
(3)
矩阵。
5m ，采用克拉索夫斯基地球
6378245m
6356863m
(4)
2.2 GPS 动态滤波
动态滤波基于当前
统计模型，在地球坐标系下解算。选取系统的状态变量
为
(5)
式中，位置误差视为有色噪声，为一阶马尔科夫过程，可表示为：
ε
τεετεετ-=-
=-1
1
(6)
斯白噪声。
(7)
(8)
系统噪声为：
(9)
量测量为纬度动态量测值、经度动态量测值、高度和三向速度量测值。由于滤波在地球
坐标系下进行，为了简便首先将纬度、经度和高度转化为三轴位置坐标值，转化方式如下：
(10)
。量测方程为：
(11)
综上，离散化的Kalman滤波方程为：
(12)
机动加速度自适应确定方法为：
2
[
π
?] ?
k
x
=
+<0
“当前”加速度
(13)
离散化量测噪声协方差阵为：diag
=
R
三、实验结果
3.1 GPS 静态滤波
图1 GPS 静态滤波前后导航轨迹图和估计误差
3.2 GPS 动态滤波
时间/s
纬度/d e g
经度
时间/s
经度/d e g
时间/s
高度/m
-5
时间/s
纬度估计误差/d e g
-5
时间/s
经度估计误差/d e g
时间/s
高度估计误差/m
时间/s
纬度/d e g
时间/s
经度/d e g
时间/s
高度/m
时间/s
速度/(m /s )
时间/s
速度/(m /s )
时间/s
速度(m /s )
-7
时间/s
纬度估计误差
-7
时间/s
经度估计误差
图2 GPS 动态滤波前后导航轨迹图和估计误差
四、实验讨论
1．简述动态模型与静态模型的区别与联系。
静态模型的速度和加速度均为0，系统静止，状态模型比较准确，模型误差较小，量测信息只有位置信息。动态模型系统的速度和加速度均发生变化，采用当前统计模型建模，相比之下，系统模型的误差较大，量测信息由位置和速度信息。静态模型是动态模型在速度和加速度均为0时的特殊情况。
2．R 阵、Q 阵，P0阵的选取对滤波精度及收敛速度有何影响，取值时应注意什么。
R 阵的取值对滤波精度的影响很大，当R 取得太大，系统就不能有效的利用量测信息对状态进行修正，因此滤波精度较低；相反，R 取得太小，系统过分依赖量测信息，无法利用状态模型有效的去除有害的量测噪声，同样降低滤波的精度。Q 阵的取值对滤波精度的影响也很大：Q 取得太大，系统就不能有效的利用状态模型对测量噪声进行修正，因此滤波精度就较低；反之，Q 取得太小，系统就会过分的依赖状态模型的精度，以致量测信息无法对状态进行有效的修正，也会降低滤波精度；只有当R 和Q 的取值恰好与使用的状态模型的精度相吻合时，才能使状态模型和量测信息都能有效的发挥作用，互相补充，得到最高的滤波精度。P0阵的取值对于可观测性良好的系统，只影响开始的滤波精度，对收敛精度影响不大，但影响收敛速度。
3）本滤波问题是否可以用最小二乘方法解决，如果可以，请阐述最小二乘方法与Kalman 滤波方法的优劣对比。
本滤波问题可以用最小二乘方法解决。最小二乘方法的最大优点是算法简单，特别是对
时间/s
高度估计误差
时间/s
东向速度估计误差
时间/s
北向速度估计误差
时间/s
天向速度估计误差
一般的最小二乘估计，根本不必知道量测误差的统计信息。但又存在使用上的局限性，该方法只能估计确定性的常值向量，而无法估计随机向量的时间过程；最小二乘的最优指标只保证了量测的误差平方和最小，而并未确保被估计量的估计误差达到最佳，因此该方法的估计精度不高。而卡尔曼滤波是一种线性最小方差估计，其算法是递推的，且使用状态空间法在时域内设计滤波器，适用于多维随机过程的估计；卡尔曼滤波的估计量可以是平稳的，也可以是非平稳的；卡尔曼滤波具有连续型和离散型两类算法，离散型算法可直接在数字计算机上实现。
（注：可编辑下载，若有不当之处，请指正，谢谢!）