卡尔曼滤波与组合导航课程报告文档格式.docx
- 文档编号:20467066
- 上传时间:2023-01-23
- 格式:DOCX
- 页数:29
- 大小:24.43KB
卡尔曼滤波与组合导航课程报告文档格式.docx
《卡尔曼滤波与组合导航课程报告文档格式.docx》由会员分享,可在线阅读,更多相关《卡尔曼滤波与组合导航课程报告文档格式.docx(29页珍藏版)》请在冰豆网上搜索。
y、
z分别为陀螺随
机常值漂移和加速度计随机常值零偏。
(下标
E、N、U分别代表东、北、天)
系统的噪声转移矩阵
G为:
Cbn
033
G
093
15
6
系统噪声矢量由陀螺仪和加速度计的随机误差组成,表达式为:
w
z
x
y
系统的状态转移矩阵
F组成内容为:
FN
FS
,其中FN中非零元素为可由惯导误差模型获得。
。
F
069FM
03
96
(2)量测方程
量测变量z
VE
VN
VU
L
,,V、V、V、L、
H
E
N
U
和H分别为捷联解算与GPS的东向速度、北向速度、天向速度、纬度、经度和高度之
差;
量测矩阵HHV
HP
036diagRM
H,(RNH)cosL,
036,
,HP
1
V
33
diag1,1,10
39,vvVEvVNvVUvv
v为量测噪声。
量测噪声方
差阵R根据GPS的位置、速度噪声水平选取。
(3)卡尔曼滤波方程
状态一步预测:
?
Xk/k1
k,k1Xk1
状态估计:
?
Kk(Zk
Xk
Xk/k1
HkXk/k1)
滤波增益:
Kk
Pk/k1HkT(HkPk/k
1HkT
Rk)1
一步预测均方误差:
Pk/k1k,k1Pk1
1Qk1
k,k
估计均方误差:
Pk
(I
KkHk)Pk/k
三、实验内容及步骤
1、实验内容
①捷联惯导/GPS组合导航系统静态导航实验;
2、实验步骤
1)实验准备(由实验教师操作)
①将IMU安装在大理石实验台上,确认IMU的安装基准面靠在大理石实验平台上的方位基准尺上;
②将IMU与导航计算机、导航计算机与稳压电源、导航计算机与监控计算机、GPS接收机与导航计算机、GPS天线与GPS接收机、GPS接收机与GPS电池之间的连接线正确连接;
③打开GPS信号转发器;
④打开监控计算机中的监控软件;
⑤打开稳压电源开关,确认稳压电源输出为28V;
⑥打开捷联惯导/GPS组合实验系统电源,实验系统开始启动,注意30s内严禁动
IMU;
⑦打开GPS接收机电源,确认通过信号转发器可以接收到4颗以上卫星;
⑧将监控软件设置为“准备”状态,准备时间5分钟;
⑨准备过程中由实验教师介绍捷联惯导/GPS组合实验系统的组成、工作原理;
2
2)捷联惯导/GPS组合导航系统静态导航实验
①实验系统准备5分钟之后,通过监控软件,将实验系统设置为“组合导航”状态;
②记录IMU的原始输出,即角增量和比力信息;
③记录数据过程中,由实验教师介绍采用卡尔曼滤波方法进行捷联惯导/GPS组合导
航的基本原理;
④记录IMU数据5分钟后,停止记录数据,利用监控计算机中的捷联惯导/GPS组
合导航软件进行静态导航解算,并显示静态导航结果;
四、实验结果与分析
1、SINS/GPS组合导航后得纬度曲线和GPS导航纬度曲线对比如下图
34.95
GPS纬度
34.9
组合导航后纬度
34.85
34.8
)34.75
度
(34.7
纬34.65
34.6
34.55
34.5
34.45
4
5
7
8
x10
2、SINS/GPS组合导航后得经度曲线和GPS导航经度曲线对比如下图
110.1
110
GPS经度
109.9
组合导航后经度
109.8
)109.7
(109.6
经109.5109.4
109.3
109.2
109.1
012345678
3、SINS/GPS组合导航后得高度曲线和GPS导航高度曲线对比如下图
3500
GPS高度
3000
组合导航后高度
2500
)2000
m
(
度高1500
1000
500
4、SINS/GPS组合导航后得东向速度曲线和GPS导航东向速度曲线对比如下图
80
GPS东向速度
60组合导航后东向速度
40
)
s/
m20
速0
向
东
-20
-40
-60
5、SINS/GPS组合导航后得北向速度曲线和GPS导航北向速度曲线对比如下图
60
GPS北向速度
40组合导航后北向速度
20
s
/
(0
速
北-20
6、SINS/GPS组合导航后得天向速度曲线和GPS导航天向速度曲线对比如下图
GPS天向速度
6组合导航后天向速度
度2
天0
-2
-4
7、SINS/GPS组合导航后航向角曲线、俯仰角曲线和横滚角曲线如下图
200
150
100
50
度0
-50
-100
-150
-200
组合导航后航向角
组合导航后俯仰角
组合导航后横滚角
12345678
8、纯惯性导航轨迹、GPS导航轨迹和SINS/GPS组合导航轨迹对比如下图
109.7
度109.6
经
109.5
109.4
纯惯性导航轨迹
GPS导航轨迹
组合导航导航轨迹
34.534.5534.634.6534.734.7534.834.8534.934.95
纬度
9、平台失准角的估计均方差曲线如下图
东向失准角方差
0.02
度0.01
北向失准角方差
天向失准角方差
0.5
10、速度和位置的估计均方差曲线如下图
东向速度误差方差北向速度误差方差
0.010.01
0.005
02468
天向速度误差方差
0.01
纬
度误差方差
0.1
0.05
经度误差方差
0.2
高度误差方差
11、陀螺漂移的估计均方差曲线和加速度计偏置的估计均方差曲线如下图
东向陀螺漂移方差
北向陀螺漂移方差
时
小0.05
天向陀螺漂移方差
东向加计偏置方差
g
μ
北向加计偏置方差
结果分析:
从仿真结果可以看出,滤波之后载体的位置和速度比GPS输出的位置和速度精度高,
载体姿态在滤波校正后结果较好,INS/GPS组合导航有效地抑制了纯惯性导航的发散,也有效地去除
了GPS量测输出的噪声。
东北天方向的速度误差均能够估计出来,天向陀螺漂移估计不出来,在动基
座情况下,东向和北向加计零偏、天向平台失准角以及东向和北向陀螺漂移均变得可观测,收敛性变
好。
可见,INS/GPS是一种较为理想的组合导航方式。
五、源程序
clear;
clc;
%载入数据
卡尔曼\IMU.dat'
);
卡尔曼\GPS.dat'
%%%%%%%%%%定义常数
e=1/298.3;
re=6378245;
wie=7.292115147e-5;
IMU_T=1/100;
GPS_T=1/20;
g0=9.7803267714;
gk1=0.00193185138639;
gk2=0.00669437999013;
LOOP=360000;
%%%%定义存储惯导解算的位置、速度、姿态和滤波后的位置、速度、姿态
velocity=zeros(LOOP,3);
position=zeros(LOOP,3);
attitude=zeros(LOOP,3);
velocity_kf=zeros(72000,3);
position_kf=zeros(72000,3);
attitude_kf=zeros(72000,3);
%%%%%%用GPS导航的初始位置和初始速度作为导航结算的初始位置和初始速度vx=0;
vy=0.0090;
vz=0.0350;
lat=34.6522*pi/180;
long=109.2496*pi/180;
h=362.2690;
%%%%%%定义四元数初值
cita=0.25097*pi/180;
%俯仰角
gama=1.78357*pi/180;
%横滚角
kesai=305.34023*pi/180;
%航向角
q=[cos(kesai/2)*cos(cita/2)*cos(gama/2)-sin(kesai/2)*sin(cita/2)*sin(gama/2);
cos(kesai/2)*sin(cita/2)*cos(gama/2)-sin(kesai/2)*cos(cita/2)*sin(gama/2);
cos(kesai/2)*cos(cita/2)*sin(gama/2)+sin(kesai/2)*sin(cita/2)*cos(gama/2);
cos(kesai/2)*sin(cita/2)*sin(gama/2)+sin(kesai/2)*cos(cita/2)*cos(gama/2)];
%%%%滤波初始状态量和滤波初始所需矩阵
k=1;
X_f=zeros(15,1);
Q=diag([(0.01*pi/(180*3600))^2,(0.01*pi/(180*3600))^2,(0.01*pi/(180*3600))^2,
(50e-6*g0)^2,(50e-6*g0)^2,(50e-6*g0)^2]);
R=diag([0.01^2,0.01^2,0.01^2,0.1^2,0.1^2,0.15^2]);
H=zeros(6,15);
p_kf=zeros(72000,15);
x_kf=zeros(72000,15);
P=diag([(60*pi/180/3600)^2,(60*pi/180/3600)^2,(30*pi/180/60)^2,0.05^2,0.05^2,0.05^2,
(2/re)^2,(2/re)^2,2^2,(0.1*pi/180/3600)^2,(0.1*pi/180/3600)^2,(0.1*pi/180/3600)^2,
fori=1:
LOOP
Rx=re/(1-e*sin(lat)^2)+h;
Ry=re/(1+2*e-3*e*sin(lat)^2)+h;
g=g0*(1+gk1*sin(lat)^2)*(1-2*h/re)/sqrt(1-gk2*sin(lat)^2);
%%%%%%四元素姿态矩阵
Cnb=[q
(1)^2+q
(2)^2-q(3)^2-q(4)^22*(q
(2)*q(3)+q
(1)*q(4))2*(q
(2)*q(4)-q
(1)*q(3));
2*(q
(2)*q(3)-q
(1)*q(4))q
(1)^2-q
(2)^2+q(3)^2-q(4)^22*(q(3)*q(4)+q
(1)*q
(2));
2*(q
(2)*q(4)+q
(1)*q(3))2*(q(3)*q(4)-q
(1)*q
(2))q
(1)^2-q
(2)^2-q(3)^2+q(4)^2];
%%%%%%%%%%%%%%%%%%捷联惯导结算
fibb=IMU(i,6:
8)'
*g;
fibn=(Cnb'
)*fibb;
%%%%求解加速度、速度和位置
ax=fibn
(1)+(2*wie*sin(lat)+vx*tan(lat)/Rx)*vy-(2*wie*cos(lat)+vx/Rx)*vz;
ay=fibn
(2)-(2*wie*sin(lat)+vx*tan(lat)/Rx)*vx+vy*vz/Ry;
az=fibn(3)+(2*wie*cos(lat)+vx/Rx)*vx+vy^2/Ry-g;
vx=ax*IMU_T+vx;
vy=ay*IMU_T+vy;
vz=az*IMU_T+vz;
lat=lat+vy/Ry*IMU_T;
long=long+vx*sec(lat)/Rx*IMU_T;
h=h+vz*IMU_T;
%%%%%%%更新四元素姿态矩阵
wiet=[0;
wie*cos(lat);
wie*sin(lat)];
wett=[-vy/Ry;
vx/Rx;
vx*tan(lat)/Rx];
wibb=(IMU(i,3:
5)'
)*pi/180/3600;
wtbb=wibb-Cnb*(wiet+wett);
angle=wtbb*IMU_T;
M=[0,-angle
(1),-angle
(2),-angle(3);
angle
(1),0,angle(3),-angle
(2);
angle
(2),-angle(3),0,angle
(1);
angle(3),angle
(2),-angle
(1),0];
q=(cos(norm(angle)/2)*eye(4)+sin(norm(angle)/2)/norm(angle)*M)*q;
q=q/norm(q);
2*(q
(2)*q(3)-q
(1)*q(4))q
(1)^2-q
(2)^2+q(3)^2-q(4)^22*(q(3)*q(4)+q
(1)*q
(2));
%%%%%%%%%%%%%%根据更新过的四元素姿态矩阵求姿态角
cita=asin(Cnb(2,3));
%俯仰角
kesai_1=atan(Cnb(2,1)/Cnb(2,2));
gama_1=atan(-Cnb(1,3)/Cnb(3,3));
%横滚角
ifCnb(2,2)==0
ifCnb(2,1)>
kesai=-pi/2;
else
kesai=pi/2;
end
elseifCnb(2,2)>
kesai=kesai_1;
9
kesai=kesai_1+pi;
kesai=kesai_1-pi;
ifCnb(3,3)==0
ifCnb(1,3)>
gama=pi/2;
gama=-pi/2;
elseifCnb(3,3)>
gama=gama_1;
gama=gama_1-pi;
gama=gama_1+pi;
%%%%%%%%%%%%存储惯导解算求的的速度、位置和姿态角velocity(i,:
)=[vx,vy,vz];
position(i,:
)=[lat/pi*180,long/pi*180,h];
attitude(i,:
)
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 卡尔 滤波 组合 导航 课程 报告
![提示](https://static.bdocx.com/images/bang_tan.gif)