GPS和INS联合导航matlab程序（带数据有用的）

本压缩文件是组合导航常用的算法
组合导航，带有数据，可以直接用

全部资料51hei下载地址：
GPS和INS联合导航matlab程序（带数据有用的）.zip (620.09 KB, 下载次数: 98)

2018-6-19 15:03 上传

点击文件名下载附件
组合导航

1. %GPS/INS无反馈位置组合 卡尔曼滤波器
   
2. 
   
3. %%%%%%%%%%%%%%%%%%
   
4. %edit by horsejun
   
5. %%%%%%%%%%%%%%%%%%
   
6. 
   
7. %每秒更新一次速度位置误差
   
8. %连续状态系统方程
   
9. %dx = F*x + G*w
   
10. %z = H*x + v
    
11. %离散状态系统方程
    
12. %x(k+1) = A*x(k) + B*w(k)
    
13. %z(k+1) = C*x(k+1) + v(k+1)
    
14. 
    
15. function [E_attitude, E_velocity, E_position, PP] = kalman_GPS_INS_position_sp_NFb(Dp, v, p, quat, Fn, Q, R, Tg, Ta, tao)
    
16. 
    
17. %输入
    
18. %Dp     量测位置误差， 作为滤波器输入,
    
19. %Dv     量测速度误差， 作为滤波器输入，
    
20. %p      ins输出位置，作为滤波器系统参数
    
21. %v      ins输出速度，作为滤波器系统参数
    
22. %fn     ins输出导航系下比力，作为滤波器参数
    
23. %quat   ins输出四元数，作为滤波器参数
    
24. %Q      系统噪声方差
    
25. %R      测量噪声方差
    
26. %Ta     加表误差漂移相关时间
    
27. %Tg     陀螺仪误差漂移相关时间
    
28. %tao    迭代步长
    
29. %%%%%%%输入向量均为行向量%%%%%%%%%%%%%
    
30. 
    
31. %输出
    
32. %E_position     位置预测值
    
33. %E-velocity     速度预测值
    
34. 
    
35. %各参数初始化
    
36. Re                 = 6378245;   %地球长半径
    
37. e                 = 1/298.257;  %地球扁率
    
38. wie         = 7.292e-5;  %地球自转角速度
    
39. 
    
40. %   东北天速度
    
41. Ve0   = v(:,1);
    
42. Vn0   = v(:,2);
    
43. Vu0   = v(:,3);
    
44. %   导航位置
    
45. L0    = p(:,1);
    
46. h0    = p(:,3);
    
47. 
    
48. %卡尔曼滤波参数初始化
    
49. PP(1:18,1:18) = diag([1/(36*57) 1/(36*57) 1/57, 0.0001 0.0001 0.0001, 0 0 1, 0.1/(57*3600) 0.1/(57*3600) 0.1/(57*3600), 0.04/(57*3600) 0.04/(57*3600) 0.04/(57*3600), 1e-4 1e-4 1e-4].^2);   %初始误差协方差阵
    
50. PP0                                         = PP;
    
51. X                                                 = zeros(18,1);  %初始状态
    
52. E_attitude                 = zeros(1,3);
    
53. E_position                 = zeros(1,3);
    
54. E_velocity                 = zeros(1,3);
    
55. 
    
56. n = size(Dp,1);
    
57. for i=1:n-1
    
58.     %参数赋值
    
59.     Ve                 = Ve0(i);
    
60.     Vn                 = Vn0(i);
    
61.     Vu                 = Vu0(i);
    
62.     L                 = L0(i);
    
63.     h                 = h0(i);
    
64.     fe                 = Fn(i,1);
    
65.     fn                 = Fn(i,2);
    
66.     fu                 = Fn(i,3);
    
67.     Rm                 = Re*(1-2*e+3*e*sin(L)^2);
    
68.     Rn                 = Re*(1-e*sin(L)^2);
    
69.     %由四元数计算姿态阵
    
70.     q                 = quat(i,:);
    
71.     Cnb         = [1-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));
    
72.                     2*(q(2)*q(3)+q(1)*q(4)), 1-2*(q(2)^2+q(4)^2),     2*(q(3)*q(4)-q(1)*q(2));
    
73.              2*(q(2)*q(4)-q(1)*q(3)), 2*(q(3)*q(4)+q(1)*q(2)), 1-2*(q(2)^2+q(3)^2)];
    
74. 
    
75.     %连续系统状态转换阵 F 的时间更新
    
76.     F            = zeros(18,18);
    
77.     F(1,2)       = wie*sin(L)+Ve*tan(L)/(Rn+h);
    
78.     F(1,3)       = -(wie*cos(L)+Ve/(Rn+h));
    
79.     F(1,5)       = -1/(Rm+h);
    
80.     F(1,9)       = Vn/(Rm+h)^2;
    
81.     F(2,1)       = -(wie*sin(L)+Ve*tan(L)/(Rn+h));
    
82.     F(2,3)       = -Vn/(Rm+h);
    
83.     F(2,4)       = 1/(Rn+h);
    
84.     F(2,7)       = -wie*sin(L);
    
85.     F(2,9)       = -Ve/(Rn+h)^2;
    
86.     F(3,1)       = wie*cos(L)+Ve/(Rn+h);
    
87.     F(3,2)       = Vn/(Rm+h);
    
88.     F(3,4)       = tan(L)/(Rn+h);
    
89.     F(3,7)       = wie*cos(L)+Ve*(sec(L)^2)/(Rn+h);
    
90.     F(3,9)       = -Ve*tan(L)/(Rn+h)^2;
    
91.     F(4,2)       = -fu;
    
92.     F(4,3)       = fn;
    
93.     F(4,4)       = Vn*tan(L)/(Rm+h)-Vu/(Rm+h);
    
94.     F(4,5)       = 2*wie*sin(L)+Ve*tan(L)/(Rn+h);
    
95.     F(4,6)       = -(2*wie*cos(L)+Ve/(Rn+h));
    
96.     F(4,7)       = 2*wie*cos(L)*Vn+Ve*Vn*sec(L)^2/(Rn+h)+2*wie*sin(L)*Vu;
    
97.     F(4,9)       = (Ve*Vu-Ve*Vn*tan(L))/(Rn+h)^2;
    
98.     F(5,1)       = fu;
    
99.     F(5,3)       = -fe;
    
100.     F(5,4)       = -2*(wie*sin(L)+Ve*tan(L)/(Rn+h));
     
101.     F(5,5)       = -Vu/(Rm+h);
     
102.     F(5,6)       = -Vn/(Rm+h);
     
103.     F(5,7)       = -(2*wie*cos(L)+Ve*(sec(L)^2)/(Rn+h))*Ve;
     
104.     F(5,9)       = (Ve^2*tan(L)+Vn*Vu)/(Rn+h)^2;
     
105.     F(6,1)       = -fn;
     
106.     F(6,2)       = fe;
     
107.     F(6,4)       = 2*(wie*cos(L)+Ve/(Rn+h));
     
108.     F(6,5)       = 2*Vn/(Rm+h);
     
109.     F(6,7)       = -2*Ve*wie*sin(L);
     
110.     F(6,9)       = -(Vn^2+Ve^2)/(Rn+h)^2;
     
111.     F(7,5)       = 1/(Rm+h);
     
112.     F(7,9)       = -Vn/(Rm+h)^2;
     
113.     F(8,4)       = 1/((Rn+h)*cos(L));
     
114.     F(8,7)       = Ve*tan(L)/((Rn+h)*cos(L));
     
115.     F(8,9)       = -Ve/(cos(L)*(Rn+h)^2);
     
116.     F(9,6)       = 1;
     
117.     F(1:3,10:12) = Cnb;
     
118.     F(1:3,13:15) = Cnb;
     
119.     F(4:6,16:18) = Cnb;
     
120.     F(13,13)     = -1/Tg(1);
     
121.     F(14,14)     = -1/Tg(2);
     
122.     F(15,15)     = -1/Tg(3);
     
123.     F(16,16)     = -1/Ta(1);
     
124.     F(17,17)     = -1/Ta(2);
     
125.     F(18,18)     = -1/Ta(3);
     
126.     %连续系统输入矩阵更新
     
127.     G                                    = zeros(18,9);
     
128.     G(1:3,1:3)   = Cnb;
     
129.     G(13:15,4:6) = eye(3,3);
     
130.     G(16:18,7:9) = eye(3,3);
     
131.     %连续系统量测阵更新
     
132.     H                                         = zeros(3,18);
     
133.     H(1,7)                         = 1;
     
134.     H(2,8)                         = 1;
     
135.     H(3,9)                         = 1;
     
136.     %连续系统离散化
     
137.     A                                   = eye(18,18)+F*tao;
     
138.     B                             = (eye(18,18)+tao*F/2)*G*tao;
     
139.    
     
140.     %卡尔曼滤波
     
141.     P                                         = A*(PP0)*A'+B*Q*B';
     
142.     K                                         = P*H'*inv(H*P*H'+R);
     
143.     PP0                                 = (eye(18,18)-K*H)*P;
     
144.     PP0                                 = (PP0+PP0')/2;
     
145.     PP(i,:)                 = diag(PP0);
     
146.    
     
147.     z = Dp(i+1,:)';
     
148.     XX = A*X+K*(z-H*A*X);
     
149.     X = XX;
     
150. 
     
151.     E_attitude(i+1,:) = XX(1:3)';
     
152.     E_velocity(i+1,:) = XX(4:6)';
     
153.     E_position(i+1,:) = XX(7:9)';
     
154. end
     
155. 
     

复制代码

1. %GPS/INS组合导航
   
2. 
   
3. %%%%%%%%%%%%%%%%%%
   
4. %edit by horsejun
   
5. %%%%%%%%%%%%%%%%%%
   
6. 
   
7. %量测信号：   位置
   
8. %INS输出数据由simulink计算得出
   
9. 
   
10. 
    
11. clear
    
12. clc
    
13. 
    
14. %得到轨迹信号
    
15. 
    
16. load ode500
    
17. 
    
18. Re                 = 6378245;                                                                                                                                                                                     %地球长半径
    
19. 
    
20. %真实轨迹
    
21. a_R         = yout(:,1:3);
    
22. v_R          = yout(:,4:6);
    
23. p_R         = yout(:,7:9);
    
24. 
    
25. %加噪声后的INS计算结果
    
26. a_ins = yout(:,10:12);
    
27. v_ins = yout(:,13:15);
    
28. p_ins = yout(:,16:18);
    
29. quat  = yout(:,19:22);    %姿态四元数
    
30. Fn    = yout(:,23:25);         %地理系下的比力
    
31. 
    
32. %惯导相关的噪声统计数据
    
33. Q_wg  = (0.04/(57*3600))^2;         %陀螺马氏过程
    
34. Q_wr  = (0.01/(57*3600))^2;              %陀螺白噪声
    
35. Q_wa  = (1e-3)^2;   %加计马氏过程
    
36. Q                 = diag([Q_wg Q_wg Q_wg,  Q_wr Q_wr Q_wr,  Q_wa Q_wa Q_wa]);
    
37. Tg                 = 300*ones(3,1);
    
38. Ta                 = 1000*ones(3,1);
    
39. 
    
40. %得到带误差的GPS输出信号
    
41. p_gps_sample = p_R(1:10:end,:);
    
42. n = size(p_gps_sample,1);
    
43. p_error(:,1:2)  = 30*randn(n,2)/Re;   
    
44. p_error(:,3) = 30*randn(n,1);                                %位置误差
    
45. p_gps = p_gps_sample+p_error;                                %加入位置误差
    
46. R = diag(std(p_error).^2);                                   %计算测量噪声方差R
    
47. 
    
48. %卡尔曼滤波
    
49. tao= 1;                                            %滤波步长
    
50. a_ins_sample                 = a_ins(1:10:end,:);
    
51. v_ins_sample                 = v_ins(1:10:end,:);
    
52. p_ins_sample                 = p_ins(1:10:end,:);
    
53. a_R_sample                         = a_R(1:10:end,:);
    
54. v_R_sample                         = v_R(1:10:end,:);
    
55. p_R_sample                         = p_R(1:10:end,:);
    
56. Dp= p_ins_sample-p_gps;                           %INS与GPS输出的位置差值
    
57. a = a_ins_sample;
    
58. v = v_ins_sample;
    
59. p = p_ins_sample;
    
60. quat0 = quat(1:10:end,:);
    
61. Fn0 = Fn(1:10:end,:);
    
62. 
    
63. [Error_a, Error_v, Error_p, PP] = kalman_GPS_INS_position_sp_NFb(Dp, v, p, quat0, Fn0, Q, R, Tg, Ta, tao);   %得到位置，速度误差误差估计值
    
64. 
    
65. a_estimate                         = a(1:size(Error_a,1),:)-Error_a;
    
66. v_estimate                         = v(1:size(Error_v,1),:)-Error_v;
    
67. p_estimate                         = p(1:size(Error_p,1),:)-Error_p;
    
68. 
    
69. n = size(p_estimate,1);   %行数
    
70. 
    
71. %位置误差比较
    
72. figure  
    
73. subplot(3,1,1)
    
74. plot((1:n),(p_R_sample(1:n,1)-p(1:n,1))*6e6,'k',(1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6e6,'r')  %黑线-滤波前的误差   红线-滤波后的误差
    
75. xlabel('时间，单位s')
    
76. subplot(3,1,2)
    
77. plot((1:n),(p_R_sample(1:n,2)-p(1:n,2))*6e6,'k',(1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6e6,'r')  %黑线-滤波前的误差   红线-滤波后的误差
    
78. ylabel('位置误差，单位m')
    
79. subplot(3,1,3)
    
80. plot((1:n),p_R_sample(1:n,3)-p(1:n,3),'k',(1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r')                                                  %黑线-滤波前的误差   红线-滤波后的误差
    
81. xlabel('黑线-滤波前的INS误差   红线-滤波后的误差')
    
82. 
    
83. %速度误差比较
    
84. figure  
    
85. subplot(3,1,1)
    
86. plot((1:n),v_R_sample(1:n,1)-v(1:n,1),'k',(1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r')                                                   %黑线-滤波前的误差   红线-滤波后的误差
    
87. xlabel('时间，单位s')
    
88. subplot(3,1,2)
    
89. plot((1:n),v_R_sample(1:n,2)-v(1:n,2),'k',(1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r')                                                  %黑线-滤波前的误差   红线-滤波后的误差
    
90. ylabel('速度误差，单位m/s')
    
91. subplot(3,1,3)
    
92. plot((1:n),v_R_sample(1:n,3)-v(1:n,3),'k',(1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r')                                                  %黑线-滤波前的误差   红线-滤波后的误差
    
93. xlabel('黑线-滤波前的INS误差   红线-滤波后的误差')
    
94. 
    
95. %位置误差
    
96. figure  
    
97. subplot(3,1,1)
    
98. xlabel('时间，单位s')
    
99. plot((1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6370000,'r')                                                                                                                                                                  %红线-滤波后的误差
    
100. subplot(3,1,2)
     
101. plot((1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6370000,'r')                                                                                                                                                                  %红线-滤波后的误差
     
102. ylabel('位置误差，单位m')
     
103. subplot(3,1,3)
     
104. plot((1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r')                                                                                                                                                                                                    %红线-滤波后的误差
     
105. xlabel('滤波后的位置误差')
     
106. 
     
107. %速度误差
     
108. figure  
     
109. subplot(3,1,1)
     
110. plot((1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r')                                                                                                                                                                                                          % 红线-滤波后的误差
     
111. xlabel('时间，单位s')
     
112. subplot(3,1,2)
     
113. plot((1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r')                                                                                                                                                                                                          %红线-滤波后的误差
     
114. ylabel('速度误差，单位m/s')
     
115. subplot(3,1,3)
     
116. plot((1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r')                                                                                                                                                                                                          % 红线-滤波后的误差
     
117. xlabel('滤波后的速度误差')
     
118. 
     

复制代码

顶帖，很赞

又看到您了