卡尔曼滤波与组合导航课程实验报告.pdf

卡尔曼滤波与组合导航课程实验报告 实验捷联惯导 /GPS 组合导航系统静态导航实验实验序号3 姓名陈星宇系院专业17 班 级ZY11172 学号ZY1117212 日期2012-5-15 指导教师宫晓琳成绩 一、实验目的 掌握捷联惯导 /GPS组合导航系统的构成和基本工作原理； 掌握采用卡尔曼滤波方法进行捷联惯导/GPS组合的基本原理； 掌握捷联惯导 /GPS组合导航系统静态性能； 了解捷联惯导 /GPS组合导航静态时的系统状态可观测性； 二、实验原理 （1）系统方程XFXGW T ENUENUxyzxyz XvvvLh 其中， E 、 N 、 U 为数学平台失准角； E v、 N v、 U v分别为载体的东向、北向和天向速度误差； L、h分别为纬度误差、经度误差和高度误差； x 、 y、z 、 x 、 y、z分别为陀螺随 机常值漂移和加速度计随机常值零偏。（下标E、N、U 分别代表东、北、天） 系统的噪声转移矩阵G为： 3 3 3 3 9 39 3 15 6 0 0 00 n b n b C GC 系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为： T xyzxyz wwwwwww 系统的状态转移矩阵F组成内容为： 6 9 0 NS M FF F F ，其中 N F中非零元素为可由惯导误差模型获得。 n b3 3 n 3 3b 3 33 3 9 6 S 0 0 00 C FC。 （2）量测方程 量测变量 T ENU zVVVLH， E V 、 N V 、 U V 、L、 和H分别为捷联解算与GPS的东向速度、北向速度、天向速度、纬度、经度和高度之 差；量测矩阵 T VP HHH， P3 6 0H N3 6 diag, ()cos, M RHRHL0， V H 3 3 0 3 9 diag 1, 1, 10， ENU T LH VVV vvvvvvv为量测噪声。 量测噪声方 差阵R根据 GPS的位置、速度噪声水平选取。 （3）卡尔曼滤波方程 状态一步预测： /1,11 ? k kk kk XX 状态估计： /1/1 ? () kk kkkkk k XXKZH X 滤波增益： 1 /1/1 () TT kk kkkk kkk KPHH PHR 一步预测均方误差： /1,11,11 T k kk kkk kk PPQ 估计均方误差： /1 () kkkk k PIK HP 三、实验内容及步骤 1、实验内容 捷联惯导 /GPS组合导航系统静态导航实验； 2、实验步骤 1） 实验准备（由实验教师操作） 将 IMU 安装在大理石实验台上，确认IMU 的安装基准面靠在大理石实验平台上 的方位基准尺上； 将 IMU 与导航计算机、 导航计算机与稳压电源、导航计算机与监控计算机、GPS 接收机与导航计算机、 GPS天线与 GPS接收机、GPS接收机与 GPS电池之间的连接线正 确连接； 打开 GPS信号转发器； 打开监控计算机中的监控软件； 打开稳压电源开关，确认稳压电源输出为28V； 打开捷联惯导 /GPS 组合实验系统电源，实验系统开始启动，注意30s 内严禁动 IMU ； 打开 GPS接收机电源，确认通过信号转发器可以接收到4 颗以上卫星； 将监控软件设置为“准备”状态，准备时间5 分钟； 准备过程中由实验教师介绍捷联惯导/GPS组合实验系统的组成、工作原理； 2） 捷联惯导 /GPS 组合导航系统静态导航实验 实验系统准备 5 分钟之后， 通过监控软件， 将实验系统设置为 “组合导航” 状态； 记录 IMU 的原始输出，即角增量和比力信息； 记录数据过程中，由实验教师介绍采用卡尔曼滤波方法进行捷联惯导/GPS组合导 航的基本原理； 记录 IMU 数据 5 分钟后，停止记录数据，利用监控计算机中的捷联惯导/GPS组 合导航软件进行静态导航解算，并显示静态导航结果； 四、实验结果与分析 1、SINS/GPS组合导航后得纬度曲线和GPS导航纬度曲线对比如下图 012345678 x 10 4 34.45 34.5 34.55 34.6 34.65 34.7 34.75 34.8 34.85 34.9 34.95 纬 度 （ 度 ） GPS 纬 度 组 合 导 航 后 纬 度 2、SINS/GPS组合导航后得经度曲线和GPS导航经度曲线对比如下图 012345678 x 10 4 109.1 109.2 109.3 109.4 109.5 109.6 109.7 109.8 109.9 110 110.1 经 度 （ 度 ） GPS 经 度 组 合 导 航 后 经 度 3、SINS/GPS组合导航后得高度曲线和GPS导航高度曲线对比如下图 012345678 x 10 4 0 500 1000 1500 2000 2500 3000 3500 高 度 （ m ） GPS高度 组 合导 航 后高度 4、SINS/GPS组合导航后得东向速度曲线和GPS导航东向速度曲线对比如下图 012345678 x 10 4 -60 -40 -20 0 20 40 60 80 东 向 速 度 （ m /s ） GPS东 向速 度 组合 导航后 东向速度 5、SINS/GPS组合导航后得北向速度曲线和GPS导航北向速度曲线对比如下图 012345678 x 10 4 -60 -40 -20 0 20 40 60 北 向 速 度 （ m /s ） GPS北 向速度 组合导航后北向速 度 6、SINS/GPS组合导航后得天向速度曲线和GPS导航天向速度曲线对比如下图 012345678 x 10 4 -4 -2 0 2 4 6 8 天 向 速 度 （ m /s ） GPS天向速度 组合导航后天向 速度 7、SINS/GPS组合导航后航向角曲线、俯仰角曲线和横滚角曲线如下图 012345678 x 10 4 -200 -150 -100 -50 0 50 100 150 200 度 组 合 导 航 后 航 向 角 组 合 导 航 后 俯 仰 角 组 合 导 航 后 横 滚 角 8、纯惯性导航轨迹、 GPS导航轨迹和 SINS/GPS组合导航轨迹对比如下图 34.4534.534.5534.634.6534.734.7534.834.8534.934.95 109.1 109.2 109.3 109.4 109.5 109.6 109.7 109.8 109.9 110 110.1 纬 度 经 度 纯 惯 性 导 航 轨 迹 GPS 导 航 轨 迹 组 合 导 航 导 航 轨 迹 9、平台失准角的估计均方差曲线如下图 012345678 x 10 4 0 0.01 0.02 东 向 失 准 角 方 差 度 012345678 x 10 4 0 0.01 0.02 北 向 失 准 角 方 差 度 012345678 x 10 4 0 0.5 天 向 失 准 角 方 差 度 10、速度和位置的估计均方差曲线如下图 02468 x 10 4 0 0.005 0.01 东 向 速度 误差 方 差 m /s 02468 x 10 4 0 0.005 0.01 北 向 速度 误 差 方 差 m /s 02468 x 10 4 0 0.005 0.01 天 向 速度 误差 方 差 m /s 02468 x 10 4 0 0.05 0.1 纬度 误 差 方 差 m 02468 x 10 4 0 0.05 0.1 经 度误 差方 差 m 02468 x 10 4 0 0.1 0.2 高度 误 差 方 差 m 11、陀螺漂移的估计均方差曲线和加速度计偏置的估计均方差曲线如下图 02468 x 10 4 0 0.05 0.1 东向陀螺漂移方差 度 /小 时 02468 x 10 4 0 0.05 0.1 北向陀螺漂移方差 度 /小 时 02468 x 10 4 0 0.05 0.1 天向陀螺漂移方差 度 /小 时 02468 x 10 4 0 50 东向加计偏置方差 g 02468 x 10 4 0 50 北向加计偏置方差 g 02468 x 10 4 0 50 北向加计偏置方差 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.012,0.012,0.012,0.12,0.12,0.152); 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.052,0.052,0.052, (2/re)2,(2/re)2,22,(0.1*pi/180/3600)2, (0.1*pi/180/3600)2,(0.1*pi/180/3600)2, (50e-6*g0)2,(50e-6*g0)2,(50e-6*g0)2); for i=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)2 2*(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)2 2*(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+vy2/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); Cnb=q(1)2+q(2)2-q(3)2-q(4)2 2*(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)2 2*(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; %根据更新过的四元素姿态矩阵求姿态角 cita=asin(Cnb(2,3); % 俯仰角 kesai_1=atan(Cnb(2,1)/Cnb(2,2); % 航向角 gama_1=atan(-Cnb(1,3)/Cnb(3,3); % 横滚角 if Cnb(2,2)=0 if Cnb(2,1)0 kesai=-pi/2; else kesai=pi/2; end elseif Cnb(2,2)0 kesai=kesai_1; else if Cnb(2,1)0 kesai=kesai_1+pi; else kesai=kesai_1-pi; end end if Cnb(3,3)=0 if Cnb(1,3)0 gama=pi/2; else gama=-pi/2; end elseif Cnb(3,3)0 gama=gama_1; else if Cnb(1,3)0 gama=gama_1-pi; else gama=gama_1+pi; end end %存储惯导解算求的的速度、位置和姿态角 velocity(i,:) = vx,vy,vz; position(i,:) = lat/pi*180,long/pi*180,h; attitude(i,:) = kesai/pi*180,cita/pi*180,gama/pi*180; %开始滤波 if(mod(i,5)=0) Rx=re/(1-e*sin(lat)2)+h; Ry=re/(1+2*e-3*e*sin(lat)2)+h; %定义量测矩阵和系统噪声驱动阵 H1=zeros(3,3),eye(3,3),zeros(3,9); H2=zeros(3,6),diag(Ry,Rx*cos(lat),1),zeros(3,6); H=H1;H2; G=Cnb',zeros(3,3);zeros(3,3),Cnb'zeros(9,3),zeros(9,3); %根据离散化求取系统的一步转移阵fai_x Fn=zeros(9,9); Fn(1,2)=wie*sin(lat)+vx*tan(lat)/Rx; Fn(1,3)=-wie*cos(lat)-vx/Rx; Fn(1,5)=-1/Ry; Fn(1,9)=vy/Ry/Ry; Fn(2,1)=-wie*sin(lat)-vx*tan(lat)/Rx; Fn(2,3)=-vy/Ry; Fn(2,4)=1/Rx; Fn(2,7)=-wie*sin(lat); Fn(2,9)=-vx/Rx/Rx; Fn(3,1)=wie*cos(lat)+vx/Rx; Fn(3,2)=vy/Ry; Fn(3,4)=tan(lat)/Rx; Fn(3,7)=wie*cos(lat)+vx*sec(lat)2/Rx; Fn(3,9)=-vx*tan(lat)/Rx/Rx; Fn(4,2)=-fibn(3); Fn(4,3)=fibn(2); Fn(4,4)=(vy*tan(lat)-vz)/Rx; Fn(4,5)=2*wie*sin(lat)+vx*tan(lat)/Rx; Fn(4,6)=-2*wie*cos(lat)-vx/Rx; Fn(4,7)=2*wie*cos(lat)*vy+2*wie*sin(lat)*vz+vx*vy*sec(lat)2/Rx; Fn(4,9)=(vx*vz-vx*vy*tan(lat)/Rx/Rx; Fn(5,1)=fibn(3); Fn(5,3)=-fibn(1); Fn(5,4)=-2*wie*sin(lat)-2*vx*tan(lat)/Rx; Fn(5,5)=-vz/Ry; Fn(5,6)=-vy/Ry; Fn(5,7)=-vx*(2*wie*cos(lat)+vx*sec(lat)2/Rx); Fn(5,9)=(vy*vz+vx*vx*tan(lat)/Rx/Rx; Fn(6,1)=-fibn(2); Fn(6,2)=fibn(1); Fn(6,4)=-2*wie*cos(lat)-2*vx/Rx; Fn(6,5)=2*vy/Ry; Fn(6,7)=-2*wie*sin(lat)*vx; Fn(6,9)=-(vx*vx+vy*vy)/Rx/Rx; Fn(7,5)=1/Ry; Fn(7,9)=-vy/Ry/Ry; Fn(8,4)=sec(lat)/Rx; Fn(8,7)=vx*tan(lat)/Rx/cos(lat); Fn(8,9)=-vx/Rx/Rx/cos(lat); Fn(9,6)=1; Fs=Cnb',zeros(3,3);zeros(3,3),Cnb'zeros(3,6); F=Fn,Fs;zeros(6,15); F=F*GPS_T; temp1=eye(15); fai_x=eye(15); for j=1:10 temp1=F*temp1/j; fai_x=fai_x+temp1; end GQG=G*Q*G' temp1=GQG*GPS_T; disB=temp1; for j = 2:11 temp2=F*temp1; temp1=(temp2+temp2')/j; disB=disB+temp1; end %滤波方程 Z=vx-GPS(k,6);vy-GPS(k,7);vz-GPS(k,8);lat-GPS(k,3);long-GPS(k,4);h-GPS(k,5); X_0=fai_x*X_f; P_0=fai_x*P*fai_x'+disB; K=P_0*H'*(inv(H*P_0*H'+R); X_f=X_0+K*(Z-H*X_0); P=(eye(15)-K*H)*P_0*(eye(15)-K*H)'+K*R*K' %存储每次循环滤波得出的状态值 x_kf(k,1)=X_f(1)*180/pi; x_kf(k,2)=X_f(2)*180/pi; x_kf(k,3)=X_f(3)*180/pi; x_kf(k,4)=X_f(4); x_kf(k,5)=X_f(5); x_kf(k,6)=X_f(6); x_kf(k,7)=X_f(7)*Ry; x_kf(k,8)=X_f(8)*Rx*cos(lat); x_kf(k,9)=X_f(9); x_kf(k,10)=X_f(10)*180*3600/pi; x_kf(k,11)=X_f(11)*180*3600/pi; x_kf(k,12)=X_f(12)*180*3600/pi; x_kf(k,13)=X_f(13)*106/g0; x_kf(k,14)=X_f(14)*106/g0; x_kf(k,15)=X_f(15)*106/g0; %计算状态的估计均方差 p_kf(k,1)=sqrt(abs(P(1,1)*180/pi; p_kf(k,2)=sqrt(abs(P(2,2)*180/pi; p_kf(k,3)=sqrt(abs(P(3,3)*180/pi; p_kf(k,4)=sqrt(abs(P(4,4); p_kf(k,5)=sqrt(abs(P(5,5); p_kf(k,6)=sqrt(abs(P(6,6); p_kf(k,7)=sqrt(abs(P(7,7)*Ry; p_kf(k,8)=sqrt(abs(P(8,8)*Rx*cos(lat); p_kf(k,9)=sqrt(abs(P(9,9); p_kf(k,10)=sqrt(abs(P(10,10)*180*3600/pi; p_kf(k,11)=sqrt(abs(P(11,11)*180*3600/pi; p_kf(k,12)=sqrt(abs(P(12,12)*180*3600/pi; p_kf(k,13)=sqrt(abs(P(13,13)*106/g0; p_kf(k,14)=sqrt(abs(P(14,14)*106/g0; p_kf(k,15)=sqrt(abs(P(15,15)*106/g0; %由滤波值对速度和位置进行输出校正 vx_kf=vx-X_f(4); vy_kf=vy-X_f(5); vz_kf=vz-X_f(6); lat_kf=lat-X_f(7); long_kf=long-X_f(8); h_kf=h-X_f(9); %求取校正后的四元素姿态阵 Acita=X_f(1); Agama=X_f(2); Akesai=X_f(3); Ctn_kf=cos(Agama)*cos(Akesai)-sin(Agama)*sin(Acita)*sin(Akesai), cos(Agama)*sin(Akesai)+sin(Agama)*sin(Acita)*cos(Akesai), -sin(Agama)*cos(Acita); -cos(Acita)*sin(Akesai), cos(Acita)*cos(Akesai), sin(Acita); sin(Agama)*cos(Akesai)+cos(Agama)*sin(Acita)*sin(Akesai), sin(Agama)*sin(Akesai)-cos(Agama)*sin(Acita)*cos(Akesai), cos(Agama)*cos(Acita); Ctb_kf=Cnb*Ctn_kf; Cnb_kf=Ctb_kf; %根据校正后的四元素姿态矩阵求取姿态角 cita_kf=asin(Cnb_kf(2,3); % 俯仰角 kesai_2=atan(Cnb_kf(2,1)/Cnb_kf(2,2); %航向角 gama_2=atan(-Cnb_kf(1,3)/Cnb_kf(3,3); % 横滚角 if Cnb_kf(2,2)=0 if Cnb_kf(2,1)0 kesai_kf=-pi/2; else kesai_kf=pi/2; end elseif Cnb_kf(2,2)0 kesai_kf=kesai_2; else if Cnb_kf(2,1)0 kesai_kf=kesai_2+pi; else kesai_kf=kesai_2-pi; end end if Cnb_kf(3,3)=0 if Cnb_kf(1,3)0 gama_kf=pi/2; else gama_kf=-pi/2; end elseif Cnb_kf(3,3)0 gama_kf=gama_2; else if Cnb_kf(1,3)0 gama_kf=gama_2-pi; else gama_kf=gama_2+pi; end end %存储校正后的速度、位置和姿态值 velocity_kf(k,:)=vx_kf,vy_kf,vz_kf; position_kf(k,:)=lat_kf*180/pi,long_kf*180/pi,h_kf; attitude_kf(k,:)=kesai_kf*180/pi,cita_kf*180/pi,gama_kf*180/pi; k=k+1; X_f(10:15)=0; end end t=1:72000; figure(1) plot(1:72001,GPS(:,3);hold on plot(t,position_kf(:,1),'r');hold on legend('GPS纬度','组合导航后纬度 ');ylabel('纬度（度） '); figure(2) plot(1:72001,GPS(:,4);hold on plot(t,position_kf(:,2),'r');hold on legend('GPS经度','组合导航后经度 ');ylabel('经度（度） '); figure(3) plot(1:72001,GPS(:,5);hold on plot(t,position_kf(:,3),'r');hold on legend('GPS高度','组合导航后高度 ');ylabel('高度（ m）'); figure(4) plot(1:72001,GPS(:,6);hold on plot(t,velocity_kf(:,1),'r');hold on legend('GPS东向速度 ','组合导航后东向速度 ');ylabel('东向速度（ m/s）'); figure(5) plot(1:72001,GPS(:,7);hold on plot(t,velocity_kf(:,2),'r');hold on legend('GPS北向速度 ','组合导航后北向速度 ');ylabel('北向速度（ m/s）'); figure(6) plot(1:72001,GPS(:,8);hold on plot(t,velocity_kf(:,3),'r');hold on legend('GPS天向速度 ','组合导航后天向速度 ');ylabel('天向速度（ m/s）') figure(7) plot(t,attitude_kf(:,1);hold on plot(t,attitude_kf(:,2),'r');hold on plot(t,attitude_kf(:,3),'g');hold on legend('组合导航后航向角 ','组合导航后俯仰角 ','组合导航后横滚角 ');ylabel('度'); figure(8) plot(position(:,1),position(:,2);hold on plot(GPS(:,3),GPS(:,4),'r');hold on plot(position_kf(:,1),position_kf(:,2),'g');hold on legend('纯惯性导航轨迹 ','GPS导航轨迹 ','组合导航导航轨迹 ');xlabel('纬度'); ylabel('经度'); figure(9) subplot(3,1,1); plot(t,p_kf(:,1);title(' 东向失准角方差 ');ylabel('度'); subplot(3,1,2); plot(t,p_kf(:,2);title(' 北向失准角方差 ');ylabel('度'); subplot(3,1,3); plot(t,p_kf(:,3);title(' 天向失准角方差 ');ylabel('度'); figure(10) subplot(3,2,1); plot(t,p_kf(:,4);title(' 东向速度误差方差 ');ylabel('m/s'); subplot(3,2,2); plot(t,p_kf(:,5);title(' 北向速度误差方差 ');ylabel('m/s'); subplot(3,2,3); plot(t,p_kf(:,6);title(' 天向速度误差方差 ');ylabel('m/s'); subplot(3,2,4); plot(t,p_kf(:,7);title(' 纬度误差方差 ');ylabel('m'); subplot(3,2,5); plot(t,p_kf(:,8);title(' 经度误差方差 ');ylabel('m'); subplot(3,2,6); plot(t,p_kf(:,9);title(' 高度误差方差 ');ylabel('m'); figure(11) subplot(3,2,1); plot(t,p_kf(:,10);title(' 东向陀螺漂移方差 ');ylabel('度/小时'); subplot(3,2,2); plot(t,p_kf(:,11);title(' 北向陀螺漂移方差 ');ylabel('度/小时'); subplot(3,2,3); plot(t,p_kf(:,12);title(' 天向陀螺漂移方差 ');ylabel('度/小时'); subplot(3,2,4); plot(t,p_kf(:,13);title(' 东向加计偏置方差 ');ylabel('g'); subplot(3,2,5); plot(t,p_kf(:,14);title(' 北向加计偏置方差 ');ylabel('g'); subplot(3,2,6); plot(t,p_kf(:,15);title(' 北向加计偏置方差 ');ylabel('g');