北航卡尔曼滤波实验报告-GPS静动态滤波实验

卡尔曼滤波实验报告

2014 年 4 月

GPS 静/动态滤波实验

一、实验要求

1、分别建立GPS 静态及动态卡尔曼滤波模型，编写程序对静态和动态GPS 数据进行Kalman 滤波。

2、对比滤波前后导航轨迹图。

3、画出滤波过程中估计均方差(P 阵对角线元素开根号)的变化趋势。

4、思考：① 简述动态模型与静态模型的区别与联系；② R 阵、Q 阵，P0阵的选取对滤波精度及收敛速度有何影响，取值时应注意什么；③ 本滤波问题是否可以用最小二乘方法解决，如果可以，请阐述最小二乘方法与Kalman 滤波方法的优劣对比。

二、实验原理

2.1 GPS 静态滤波

选取系统的状态变量为[ ]T

L 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 阵为：

2

2

25180()0

05180

(

)0cos()00

5p e R R L ππ

⨯⎛⎫ ⎪⨯ ⎪ ⎪⨯= ⎪⨯⨯ ⎪ ⎪ ⎪⎝

⎭

R (4)

2.2 GPS 动态滤波

动态滤波基于当前统计模型，在地球坐标系下解算。选取系统的状态变量为

T

x 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 x

x

y

y y y

z z z

z w w w εετεετεετ⎧=-+⎪⎪⎪⎪=-+⎨⎪⎪⎪=-+⎪⎩

1

11

(6)

其中，i τ(,,i x y z =)为对应马尔科夫过程的相关时间常数，(,,)i w i x y z =为零均值高斯白噪声。

系统矩阵A 可表示为：

444444444444⨯⨯⨯⨯⨯⨯⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦

000000x y z A A A A

(7)

其中，0

1000

01000/0

/ττ⎡⎤⎢⎥

⎢

⎥=⎢⎥-⎢⎥-⎢⎥⎣⎦

11i i a i A (i =x,y,z ) 输入量U 可表示为：

T

()000000000x y z

y 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)

所以，滤波的量测量为三轴位置坐标值和三轴速度测量值，即[ ]T

x y z Z x y z v v v =。

量测方程为：

Z =HX +V

(11)

式中，⎡⎤⎢⎥⎢⎥

⎢⎥=⎢

⎥⎢⎥

⎢⎥⎢⎥⎣⎦

1001000000000000100100000

00000001001010000000000000001000000000000000100H ，V

为零均值高斯白噪声。 综上，离散化的Kalman 滤波方程为：

1/1/11/1/111111/111/111/1/1/1111/()()()k k k

k 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/000000

x k k

y k

k k k

z k k ++++'⎡⎤

⎢⎥

''=⎢⎥⎢⎥'⎣

⎦

ΦΦΦΦ，21//1

/20010(,,)00100

i i k

k T T T T i x y z e τ+-⎡⎤⎢⎥⎢⎥

'==⎢⎥⎢⎥⎣⎦

Φ ，1//1/1/000000

x k k

y k k

k k

z k k ++++⎡⎤⎢⎥

=⎢⎥⎢⎥⎣

⎦

1Φ

ΦΦΦ，/2

/1///1

(/1)001(1)0(,,)000000

a i

i i

a i

i

a i

i T a a

T i a k k T T T T e e i x y z e

e τττττττ--+--⎡⎤-+⎢⎥⎢⎥

-==⎢⎥⎢⎥⎢⎥⎢⎥

⎣

⎦Φ 离散化的系统噪声协方差阵为：222222

000000x y z a x a y a z diag δδδδδδ⎡⎤=⎣

⎦

Q ，

机动加速度自适应确定方法为：

222max 1/max 22max 4π4πˆˆ[][] ππ4πˆ[] ? πx

x a k k k a k a x a x a x δδ+---⎧=-=-⎪⎨-⎪=+<⎩

00“当前”加速度>“当前”加速度 (13)

离散化量测噪声协方差阵为：2

2

2

x y z x y z v v v diag R R R R R R ⎡⎤=⎣

⎦

2

2

2

R 。

三、实验结果

3.1 GPS 静态滤波