北航惯性导航综合实验五实验报告Word格式.docx

1、短时段内（t5min）,忽略地球自转,运动轨迹近似为平面,此时的位置误差分析可简化为：（1）加速度计零偏引起的位置误差：m（2）失准角引起的误差：（3）陀螺漂移可得1min后的位置误差值2、一分钟惯导实验数据验证结果：（1）纯惯导解算1min的位置及位置误差图：（2）纯惯导解算1min的速度及速度误差图：实验结果分析：纯惯导解算短时间内精度很高,1min的惯导解算的北向最年夜位移误差-2.668m,东向最年夜位移误差-8.231m,可见实验数据所得位置误差与理论推导的位置误差在同一数量级,结果不完全相同是因为理论推导时做了年夜量简化,而且实验时视GPS为真实值也会带来误差；另外,可见1min内

2、纯惯导解算的东向速度最年夜误差-0.2754m/s,北向速度最年夜误差-0.08027m/s.（二）选取IMU前5分钟数据进行瞄准实验.将初始瞄准结果作为初值完成1小时捷联惯导和组合导航解算,比较1小时捷联惯导和组合导航结果.1、5minIMU数据的解析粗瞄准结果：2、5minIMU数据的Kalman滤波精瞄准结果：3、一小时IMU/GPS数据的组合导航结果图及估计方差P阵图：4、一小时IMU数据的捷联惯导解算结果与组合滤波、GPS输出比较图：5、结果分析：由滤波结果图可以看出：（1）由组合后的速度、位置的P阵可以看出滤波之后载体的速度和位置比GPS输出的精度高.（2）短时间内SINS的精度较

3、高,初始阶段的导航结果基本和GPS、组合导航结果重合,1小时后的捷联惯导解算结果很差,纬度、经度、高度均发散.（3）INS/GPS组合滤波的结果和GPS的输出结果十分近似,因为1小时的导航GPS的精度比SINS导航的精度高很多,Kalman滤波器中GPS信号的权重更年夜.（4）总体看来,SINS/GPS组合滤波的结果优于独自用SINS或GPS导航的结果,起到了协调、超越、冗余的作用,使导航系统更可靠.六、SINS/GPS组合导航法式%INS/GPS组合导航跑车1h实验%该法式为15维状态量,6维观丈量的kalman滤波法式,惯性/卫星组合松耦合的数学模型clearclcclose all%初始

4、量界说wie = 0.000072921151467;Re = 6378135.072;g = 9.7803267714;e = 1.0 / 298.25;T = 0.01; %IMU频率100hz,此法式中GPS频率100hzdatanumber = 360000; %数据时间3600s a = load（imu_1h.dat）;w = a（:,3:5）*pi/180/3600; %陀螺仪输出的角速率信息单元由/h化为rad/sf = a（:,6:8）; %三轴比力输出,单元ggps_1h_new.datgps_pos = a（:5）; %GPS输出的纬度、经度、高度信息gps_pos（:,

5、1:2） = gps_pos（:2）*pi/180; %纬经单元化为弧度gps_v = a（:8）; %GPS输出的西南天速度信息%捷联解算及卡尔曼相关v=zeros（datanumber,3）; %组合后的速度信息atti = zeros（datanumber,3）; %组合后的姿态信息pos = zeros（datanumber,3）; %组合后的位置信息gyro=zeros（3,1）;acc=zeros（3,1）;x_kf = zeros（datanumber,15）;p_kf = zeros（datanumber,15）;lon =116.3703629769560*pi/180;he

6、ight =43.0674;fai = 219.9744642380873*pi/180;theta = -0.895865732956914*pi/180;gama = 0.640089448357591*pi/180;Vx=gps_v（1,1）;Vy=gps_v（1,2）;Vz=gps_v（1,3）;X_o=zeros（15,1）; %X的初值选为0X=zeros（15,1）;%Q=diag（50e-6*g）2,（50e-6*g）2,（50e-6*g）2,（0.1*pi/180/3600）2,（0.1*pi/180/3600）2,（0.1*pi/180/3600）2,0,0,0,0,0,0

7、,0,0,0）; %随机Q=diag（0.008*pi/180/3600）2,（0.008*pi/180/3600）2,（0.008*pi/180/3600）2,（50e-6*g）2,（50e-6*g）2,（50e-6*g）2,0,0,0,0,0,0,0,0,0）;R=diag（0.01）2,（0.01）2,（0.01）2,（0.1）2,（0.1）2,（0.15）2）;P=zeros（15）;P_k=diag（0.00005*pi/180）2,（0.00005*pi/180）2,（0.00005*pi/180）2,0.000052,0.000052,0.000052,22,22,22,（0.0

8、01*pi/180/3600）2,（0.001*pi/180/3600）2,（0.001*pi/180/3600）2,（50e-6*g）2,（50e-6*g）2,（50e-6*g）2）; %K=zeros（15,6）;Z=zeros（6,1）;I=eye（15）;Cnb = cos（gama）*cos（fai）-sin（gama）*sin（theta）*sin（fai）, cos（gama）*sin（fai）+sin（gama）*sin（theta）*cos（fai）, -sin（gama）*cos（theta）; -cos（theta）*sin（fai）, cos（theta）*cos（fa

9、i）, sin（theta）; sin（gama）*cos（fai）+cos（gama）*sin（theta）*sin（fai）, sin（gama）*sin（fai）-cos（gama）*sin（theta）*cos（fai）, cos（gama） * cos（theta）;q = cos（fai/2）*cos（theta/2）*cos（gama/2） - sin（fai/2）*sin（theta/2）*sin（gama/2）; cos（fai/2）*sin（theta/2）*cos（gama/2） - sin（fai/2）*cos（theta/2）*sin（gama/2）; cos（fai

10、/2）*cos（theta/2）*sin（gama/2） + sin（fai/2）*sin（theta/2）*cos（gama/2）; cos（fai/2）*sin（theta/2）*sin（gama/2） + sin（fai/2）*cos（theta/2）*cos（gama/2）;Cnb_s=Cnb;q_s=q;for i=1:1:datanumber Rmh = Re * （1.0 - 2.0 * e + 3.0 * e * sin（lat） * sin（lat） + height; Rnh = Re * （1.0 + e * sin（lat） * sin（lat） + height; W

11、ien = 0; wie * cos（lat）; wie * sin（lat）; Wenn = -Vy / Rmh; Vx / Rnh; Vx * tan（lat） / Rnh; Winn = Wien + Wenn; Winb = Cnb * Winn; for j=1:3 gyro（j,1） = w（j,i）; acc（j,1） = f（j,i）*g; %加速度信息,单元化为m/s2 end angle = （gyro - Winb） * T; fn = Cnb* acc; difVx = fn（1） + （2.0 * wie * sin（lat） + Vx * tan（lat） / Rn

12、h） * Vy; difVy = fn（2） - （2.0 * wie * sin（lat） + Vx * tan（lat） / Rnh） * Vx; difVz = fn（3） + （2.0 * wie * cos（lat） + Vx / Rnh） * Vx + Vy * Vy / Rmh -g; Vx = difVx * T + Vx; Vy = difVy * T + Vy; Vz = difVz * T + Vz; lat = lat + Vy * T / Rmh; lon = lon + Vx * T / Rnh / cos（lat）; height = height + Vz *

13、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）*q（1）+q（2）*q（2）-q（3）*q（3）-q（4）*q（4）, 2*（q（2）*q

14、（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）*q（1）-q（2）*q（2）+q（3）*q（3）-q（4）*q（4）, 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）*q（1）-q（2）*q（2）-q（3）*q（3）+q（4）*q（4）; %以上为纯惯导解算 % F1=0 wie*sin（lat）+v（i,1）*tan（lat）/（Rnh） -（wie*cos（lat）+v（i,1）/（Rnh） 0 -

15、1/（Rmh） 0 0 0 0; -（wie*sin（lat）+v（i,1）*tan（lat）/（Rnh） 0 -v（i,2）/（Rmh） 1/（Rnh） 0 0 -wie*sin（lat） 0 0; wie*cos（lat）+v（i,1）/（Rnh） v（i,2）/（Rmh） 0 tan（lat）/（Rnh） 0 0 wie*cos（lat）+v（i,2）*sec（lat）*sec（lat）/（Rnh） 0 0; 0 -fn（3） fn（2） v（i,2）*tan（lat）/（Rmh）-v（i,3）/（Rmh） 2*wie*sin（lat）+v（i,1）*tan（lat）/（Rnh） -（2

16、*wie*cos（lat）+v（i,1）/（Rnh） （2*wie*cos（lat）*v（i,2）+v（i,1）*v（i,2）*sec（lat）*sec（lat）/（Rnh）+2*wie*sin（lat）*v（i,3） 0 0; fn（3） 0 -fn（1） -2*（wie*sin（lat）+v（i,1）*tan（lat）/（Rnh） -v（i,3）/（Rmh） -v（i,2）/（Rmh） -（2*wie*cos（lat）+v（i,1）*sec（lat）*sec（lat）/（Rnh）*v（i,1） 0 0; -fn（2） fn（1） 0 2*（wie*cos（lat）+v（i,1）/（Rnh）

17、 2*v（i,2）/（Rmh） 0 -2*wie*sin（lat）*v（i,1） 0 0; 0 0 0 0 1/（Rmh） 0 0 0 0; 0 0 0 sec（lat）/（Rnh） 0 0 v（i,1）*sec（lat）*tan（lat）/（Rnh） 0 0; 0 0 0 0 0 1 0 0 0 ; G=Cnb,zeros（3）; zeros（3）,Cnb zeros（9,6）; H=zeros（3）,eye（3）,zeros（3）,zeros（3,6）; zeros（3）,zeros（3）,diag（Rmh,Rnh*cos（lat）,1）,zeros（3,6）; %量测阵 F2=-Cnb

18、zeros（3）,zeros（3）; F=F1,F2; zeros（6,15）; %以上为kalman滤波模型参数 F = F * T; %离散化 temp1 = eye（15）; disF = eye（15）; for j = 1:10 temp1 = F * temp1 / j; disF = disF + temp1; temp1 = Q * T; disQ = temp1; for j = 2:11 temp2 = F * temp1; temp1 = （temp2 + temp2）/j; disQ = disQ + temp1; end Z（1） = Vx - gps_v（i,1）;

19、 %量丈量为纯惯导与GPS的速度差、位置差 Z（2） = Vy - gps_v（i,2）; Z（3） = Vz - gps_v（i,3）; Z（4） = （lat - gps_pos（i,1） * Rmh; %纬经度化为位移,单元m Z（5） = （lon - gps_pos（i,2） * Rnh * cos（lat）; Z（6） = height - gps_pos（i,3）; X = disF * X_o; %kalman滤波五个公式 P = disF * P_k * disF+ disQ; K = P * H/（ H * P * H+ R）; X_k = X + K * （Z - H *

20、 X）; P_k = （I - K * H） * P; x_kf（i,1） = X_k（1）/pi*180; %平台误差角 x_kf（i,2） = X_k（2）/pi*180; x_kf（i,3） = X_k（3）/pi*180; x_kf（i,4） = X_k（4）; %速度误差 x_kf（i,5） = X_k（5）; x_kf（i,6） = X_k（6）; x_kf（i,7） = X_k（7）; %位置误差 x_kf（i,8） = X_k（8）; x_kf（i,9） = X_k（9）; x_kf（i,10） = X_k（10）/pi*180*3600; %陀螺随机常值漂移,单元/h x_k

21、f（i,11） = X_k（11）/pi*180*3600; x_kf（i,12） = X_k（12）/pi*180*3600; x_kf（i,13） = X_k（13）*106/g; %加计随机常值偏置,单元ug x_kf（i,14） = X_k（14）*106/g; x_kf（i,15） = X_k（15）*106/g; p_kf（i,1） = sqrt（abs（P_k（1,1）/pi*180; p_kf（i,2） = sqrt（abs（P_k（2,2）/pi*180; p_kf（i,3） = sqrt（abs（P_k（3,3）/pi*180; p_kf（i,4） = sqrt（abs（P

22、_k（4,4）; p_kf（i,5） = sqrt（abs（P_k（5,5）; p_kf（i,6） = sqrt（abs（P_k（6,6）; p_kf（i,7） = sqrt（abs（P_k（7,7）; p_kf（i,8） = sqrt（abs（P_k（8,8）; p_kf（i,9） = sqrt（abs（P_k（9,9）; p_kf（i,10） = sqrt（abs（P_k（10,10）/pi*180*3600; p_kf（i,11） = sqrt（abs（P_k（11,11）/pi*180*3600; p_kf（i,12） = sqrt（abs（P_k（12,12）/pi*180*3600

23、; p_kf（i,13） = sqrt（abs（P_k（13,13）*106/g; p_kf（i,14） = sqrt（abs（P_k（14,14）*106/g; p_kf（i,15） = sqrt（abs（P_k（15,15）*106/g; Vx = Vx - X_k（4）; %速度校正 Vy = Vy - X_k（5）; Vz = Vz - X_k（6）; v（i,:） = Vx, Vy, Vz; lat = lat - X_k（7）; %位置校正 lon = lon - X_k（8）; height = height - X_k（9）; pos（i,:） = lat, lon, heig

24、ht; Atheta = X_k（1）; %kalman滤波估计得出的失准角theta Agama = X_k（2）; %kalman滤波估计得出的失准角gama Afai = X_k（3）; %kalman滤波估计得出的失准角fai Ctn = 1, Afai, -Agama; -Afai, 1, Atheta; Agama, -Atheta, 1; Cnb = Cnb*Ctn; %更新姿态阵 fai = atan（-Cnb（2,1） / Cnb（2,2）; theta = asin（Cnb（2,3）; gama = atan（-Cnb（1,3） / Cnb（3,3）; if （Cnb（2,

25、2） 0） fai = fai + pi; elseif （fai 0） fai = fai + 2*pi; if （Cnb（3,3） gama = gama - pi; else gama = gama + pi; atti（i,:） = fai/pi*180, theta/pi*180, gama/pi*180; q（2） = sqrt（abs（1 + Cnb（1,1） - Cnb（2,2） - Cnb（3,3） / 2; q（3） = sqrt（abs（1 - Cnb（1,1） + Cnb（2,2） - Cnb（3,3） / 2; q（4） = sqrt（abs（1 - Cnb（1,1）

26、 - Cnb（2,2） + Cnb（3,3） / 2; q（1） = sqrt（abs（1 - q（2） * q（2） - q（3） * q（3） - q（4） * q（4）; if （Cnb（2,3） Cnb（3,2） q（2） = - q（2）; if （Cnb（3,1） Cnb（1,3） q（3） = - q（3）; if （Cnb（1,2） Cnb（2,1） q（4） = - q（4）; X_k（1:9） = 0; X_o=X_k; iend%绘图%t=1:datanumber;figure（1）subplot（311）;plot（t,pos（:,1）*180/pi,r,t,gps_pos（:b）title（纬度xlabel（0.01sylabel（subplot（312）;,2）*180/pi,经度subplot（313）;,3）,高度mlegend（组合滤波,GPSfigure（2）plot（t,v（:,1）,t,gps_v（:东向速度m/ssu