卡尔曼滤波实验报告2014 年 4 月GPS 静/动态滤波实验一、实验要求1、分别建立GPS 静态及动态卡尔曼滤波模型,编写程序对静态和动态GPS 数据进行Kalman 滤波。
2、对比滤波前后导航轨迹图。
3、画出滤波过程中估计均方差(P 阵对角线元素开根号)的变化趋势。
4、思考:① 简述动态模型与静态模型的区别与联系;② R 阵、Q 阵,P0阵的选取对滤波精度及收敛速度有何影响,取值时应注意什么;③ 本滤波问题是否可以用最小二乘方法解决,如果可以,请阐述最小二乘方法与Kalman 滤波方法的优劣对比。
二、实验原理2.1 GPS 静态滤波选取系统的状态变量为[ ]TL h λ=X ,其中L 为纬度(deg),λ为经度(deg),h 为高度(m)。
设()w t 为零均值高斯白噪声,则系统的状态方程为:310()w t ⨯=+X(1)所以离散化的状态模型为:,111k k k k k W ---=+X X Φ(2)式中,,1k k -Φ为33⨯单位阵,k W 为系统噪声序列。
测量数据包括:纬度静态量测值、经度静态量测值和高度构成31⨯矩阵Z ,量测方程可以表示为:k k k Z HX V =+(3)式中,H 为33⨯单位阵,k V 为量测噪声序列。
系统的状态模型是十分准确的,所以系统模型噪声方差阵可以取得十分小,取Q 阵零矩阵。
系统测量噪声方差阵R 由测量确定,由于位置量测精度为5m ,采用克拉索夫斯基地球椭球模型,长半径e R 为6378245m ,短半径p R 为6356863m 。
所以R 阵为:2225180()005180()0cos()005p e R R L ππ⨯⎛⎫ ⎪⨯ ⎪ ⎪⨯= ⎪⨯⨯ ⎪ ⎪ ⎪⎝⎭R (4)2.2 GPS 动态滤波动态滤波基于当前统计模型,在地球坐标系下解算。
选取系统的状态变量为Tx x x y y y z z z X x v a y v a z v a εεε⎡⎤=⎣⎦,其中,,,x x x x v a ε依次为地球坐标系下x轴上的位置、速度、加速度和位置误差分量,,y z 轴同理。
系统的状态模型可以表示为:()()()()t t t t =++X AX U W(5)式中,位置误差视为有色噪声,为一阶马尔科夫过程,可表示为:x x xxyy y yz z zz w w w εετεετεετ⎧=-+⎪⎪⎪⎪=-+⎨⎪⎪⎪=-+⎪⎩111(6)其中,i τ(,,i x y z =)为对应马尔科夫过程的相关时间常数,(,,)i w i x y z =为零均值高斯白噪声。
系统矩阵A 可表示为:444444444444⨯⨯⨯⨯⨯⨯⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦000000x y z A A A A(7)其中,0100001000/0/ττ⎡⎤⎢⎥⎢⎥=⎢⎥-⎢⎥-⎢⎥⎣⎦11i i a i A (i =x,y,z ) 输入量U 可表示为:T()000000000x y zy x z a a a a a a t τττ⎡⎤=⎢⎥⎢⎥⎣⎦U(8)式中,(,,)i a i x y z =为机动加速度的当前均值,其自适应确定方法为:,/1ˆx k k k a x -=,同理可得,,y k z k a a 、。
系统噪声为:T()000000x y z a x a y a z t w w w w w w ⎡⎤=⎣⎦W (9)量测量为纬度动态量测值、经度动态量测值、高度和三向速度量测值。
由于滤波在地球坐标系下进行,为了简便首先将纬度、经度和高度转化为三轴位置坐标值,转化方式如下:(Re )cos()cos()(Re )cos()sin()(Re )sin()x h L y h L z h L λλ=+⎧⎪=+⎨⎪=+⎩(10)所以,滤波的量测量为三轴位置坐标值和三轴速度测量值,即[ ]Tx y z Z x y z v v v =。
量测方程为:Z =HX +V(11)式中,⎡⎤⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦100100000000000010010000000000001001010000000000000001000000000000000100H ,V为零均值高斯白噪声。
综上,离散化的Kalman 滤波方程为:1/1/11/1/111111/111/111/1/1/1111/()()()k k kk k k k k k k k k k T T k k k k k k k k k T k k k k k k k k k k k k k ΛΛ++ΛΛΛ++++++-++++++++++++++⎧'=⎪⎪=+-⎪⎪⎪=+⎨⎪⎪=+⎪⎪=-⎪⎩X ΦX X X K Z H X K P H H P H R P ΦP ΦQ P I K H P (12)式中,1/1/1/1/000000x k ky kk k kz k k ++++'⎡⎤⎢⎥''=⎢⎥⎢⎥'⎣⎦ΦΦΦΦ,21//1/20010(,,)00100i i kk T T T T i x y z e τ+-⎡⎤⎢⎥⎢⎥'==⎢⎥⎢⎥⎣⎦Φ ,1//1/1/000000x k ky k kk kz k k ++++⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦1ΦΦΦΦ,/2/1///1(/1)001(1)0(,,)000000a ii ia iia ii T a aT i a k k T T T T e e i x y z ee τττττττ--+--⎡⎤-+⎢⎥⎢⎥-==⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦Φ 离散化的系统噪声协方差阵为:222222000000x y z a x a y a z diag δδδδδδ⎡⎤=⎣⎦Q ,机动加速度自适应确定方法为:222max 1/max 22max 4π4πˆˆ[][] ππ4πˆ[] ? πxx a k k k a k a x a x a x δδ+---⎧=-=-⎪⎨-⎪=+<⎩00“当前”加速度>“当前”加速度 (13)离散化量测噪声协方差阵为:222x y z x y z v v v diag R R R R R R ⎡⎤=⎣⎦222R 。
三、实验结果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 阵的取值对滤波精度的时间/s高度估计误差时间/s东向速度估计误差时间/s北向速度估计误差时间/s天向速度估计误差影响也很大:Q取得太大,系统就不能有效的利用状态模型对测量噪声进行修正,因此滤波精度就较低;反之,Q取得太小,系统就会过分的依赖状态模型的精度,以致量测信息无法对状态进行有效的修正,也会降低滤波精度;只有当R和Q的取值恰好与使用的状态模型的精度相吻合时,才能使状态模型和量测信息都能有效的发挥作用,互相补充,得到最高的滤波精度。
P0阵的取值对于可观测性良好的系统,只影响开始的滤波精度,对收敛精度影响不大,但影响收敛速度。
3)本滤波问题是否可以用最小二乘方法解决,如果可以,请阐述最小二乘方法与Kalman 滤波方法的优劣对比。
本滤波问题可以用最小二乘方法解决。
最小二乘方法的最大优点是算法简单,特别是对一般的最小二乘估计,根本不必知道量测误差的统计信息。
但又存在使用上的局限性,该方法只能估计确定性的常值向量,而无法估计随机向量的时间过程;最小二乘的最优指标只保证了量测的误差平方和最小,而并未确保被估计量的估计误差达到最佳,因此该方法的估计精度不高。
而卡尔曼滤波是一种线性最小方差估计,其算法是递推的,且使用状态空间法在时域内设计滤波器,适用于多维随机过程的估计;卡尔曼滤波的估计量可以是平稳的,也可以是非平稳的;卡尔曼滤波具有连续型和离散型两类算法,离散型算法可直接在数字计算机上实现。