aided_ins_15_state.m

Upload User: wctenglong
Upload Date: 2007-05-18
Package Size: 4k
Code Size: 10k
Category: matlab
Development Platform: Matlab
  1. %
  2. %   aided_ins_15_state.m
  3. %
  4. %   Inertial Navigation System with 15-state Kalman filter using
  5. %   position-aiding
  6. %
  7. %   This program performs the integration Kalman filtering and
  8. %   thus takes input from programs which have already generated
  9. %   the INS data.  See the LOAD command below.
  10. %
  11. %   The state vector is:
  12. %    x1 = delta x position
  13. %    x2 = delta y position
  14. %    x3 = delta z position
  15. %    x4 = delta x velocity
  16. %    x5 = delta y velocity
  17. %    x6 = delta z velocity
  18. %    x7 = psi-angle x component
  19. %    x8 = psi-angle y component
  20. %    x9 = psi-angle z component
  21. %   x10 = body-x accel bias
  22. %   x11 = body-y accel bias
  23. %   x12 = body-z accel bias
  24. %   x13 = body-x gyro bias
  25. %   x14 = body-y gyro bias
  26. %   x15 = body-z gyro bias
  27. %
  28. %    Note:  the position and velocity errors are expressed in the
  29. %           so-called local-level frame or simply L (or l) frame.
  30. %           In the L-frame:  x=east; y=north; z=up
  31. %
  32. %  August 2005
  33. %  Copyright (c) 2005 by GPSoft LLC
  34. %  All Rights Reserved.
  35. clear all
  36. close all
  37. fly_over_rms = 3;  % RMS error (in meters) of the external 2-D position
  38. %                  % fix (in each dimension); also known as a fly-over fix
  39. load dyn_flt_ins_dat    % output from the program DYN_FLIGHT_INS.M
  40. randn('state',0)
  41. C = [0 1 0; 1 0 0; 0 0 -1];    % conversion between NED and ENU
  42. H = zeros(2,15);  H(1,1) = 1;  H(2,2) = 1;
  43. %                                 % The measurement vector z consists of
  44. %                                 % an x and y position difference only.
  45. %                                 % Since we are working in the L frame
  46. %                                 % this corresponds to east and north
  47. %                                 % position difference (since alpha=0).
  48. Peast_pos = fly_over_rms^2;  Pnorth_pos = fly_over_rms^2;  Pup_pos = 0;
  49. Peast_vel = 2^2;  Pnorth_vel = 2^2;  Pup_vel = 0;
  50. Ppsi_x = 0.0001^2;  Ppsi_y = 0.0001^2;  Ppsi_z = 0.0001^2;
  51. Pacc_x = (100*9.81e-6)^2; Pacc_y = (100*9.81e-6)^2; Pacc_z = (100*9.81e-6)^2;
  52. Pgyr_x = (0.05)^2; Pgyr_y = (0.05)^2; Pgyr_z = (0.05)^2;
  53. P_pre = zeros(15,15);
  54. P_pre(1,1)=Peast_pos; P_pre(2,2)=Pnorth_pos; P_pre(3,3)=Pup_pos;
  55. P_pre(4,4)=Peast_vel; P_pre(5,5)=Pnorth_vel; P_pre(6,6)=Pup_vel;
  56. P_pre(7,7)=Ppsi_x; P_pre(8,8)=Ppsi_y; P_pre(9,9)=Ppsi_z;
  57. P_pre(10,10)=Pacc_x; P_pre(11,11)=Pacc_y; P_pre(12,12)=Pacc_z;
  58. P_pre(13,13)=Pgyr_x; P_pre(14,14)=Pgyr_y; P_pre(15,15)=Pgyr_z;
  59. P_pre_KF = P_pre;   % initial prediction error covariance matrix
  60. R = [fly_over_rms^2 0; 0 fly_over_rms^2];  % measurement error covariance matrix
  61. sigma_acc = 0.3*9.81e-6;
  62. sigma_gyr = 1e-9;
  63. G = zeros(15,15); 
  64. G(10,10)=sigma_acc; G(11,11)=sigma_acc; G(12,12)=sigma_acc; 
  65. G(13,13)=sigma_gyr; G(14,14)=sigma_gyr; G(15,15)=sigma_gyr; 
  66. W = zeros(15,15); 
  67. W(10,10)=1; W(11,11)=1; W(12,12)=1;
  68. W(13,13)=1; W(14,14)=1; W(15,15)=1;
  69. tau_accel = 100000;
  70. tau_gyro = 100000;
  71. update = 0;  count = 0;
  72. X_pre = zeros(15,1);
  73. est_lat_KF(1) = lat_prof(1);   % INITIALIZATION IN THIS SECTION
  74. est_lon_KF(1) = lon_prof(1);
  75. est_alpha_KF(1) = 0;
  76. est_vel_l_KF(1,:) =  vel_l(1,:);
  77. est_roll_KF(1) = est_roll(1);
  78. est_pitch_KF(1) = est_pitch(1);
  79. est_yaw_KF(1) = est_yaw(1);
  80. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  81. h = waitbar(0,' Time Loop ');
  82. for i = 2:npts,
  83.     
  84.     % Generate the inertial error model system dynamics matrix
  85.     F_ins = F_ins_gen(est_lat(i),del_Vl(:,i),tdint(i),...
  86.         omega_el_L(:,i),omega_ie_L(:,i),g_extr(i),est_height(i,1));
  87.     % Generate the system dynamics matrix for the whole filter
  88.     F = zeros(15,15); F(1:9,1:9) = F_ins; 
  89.     F(10:12,10:12) = (-1/tau_accel)*eye(3);  F(4:6,10:12) = C*est_DCMbn(:,:,i);
  90.     F(13:15,13:15) = (-1/tau_gyro)*eye(3);  F(7:9,13:15) = C*est_DCMbn(:,:,i);
  91.     
  92.     % Generate the system noise covariance matrix and the state transition
  93.     % matrix
  94.     [Q,PHI] = q_gen(tdint(i),F,G,W,15);
  95.     % Calculate the Kalman gain
  96.     K = P_pre_KF*H'*inv(H*P_pre_KF*H' + R);
  97.     
  98.    % keep track of elapsed time (in seconds) since last Kalman update
  99.    count = count + tdint(i);
  100.     %%if count >= 630,   % 10.5 minute intervals (i.e., 1/8 Schuler cycle)
  101.     %%if count >= 60,  % 1 minute intervals
  102.     if count >= 1,    % 1 Hz updates
  103.         update = 1;
  104.         count = 0;
  105.     end
  106.     
  107.     if update == 1,
  108.         % Generate the free-inertial estimates of lat, long and ECEF x-y-z
  109.         ins_lat = lat_prof(i) + est_lat_err(i);
  110.         ins_lon = lon_prof(i) + est_lon_err(i);
  111.         est_pos_xyz = llh2xyz([ins_lat ins_lon height_prof(i)]);
  112.         % Calcuate the ECEF x-y-z coordinates of the true position
  113.         tru_pos_xyz = llh2xyz([lat_prof(i) lon_prof(i) height_prof(i)]);
  114.         % Calculate the free-inertial position error in east-north-up
  115.         % coordinates
  116.         ins_pos_err_enu = xyz2enu(est_pos_xyz,tru_pos_xyz);
  117.         % Simulate the errors of the position aiding source
  118.         fly_over_fix_errors = fly_over_rms*randn(2,1);
  119.         % Simulate the position difference between INS and position aiding
  120.         % source
  121.         Z = ins_pos_err_enu(1:2,1) + fly_over_fix_errors;  
  122.         
  123.         % Compute Kalman estimate
  124.         X_est = X_pre + K*(Z - H*X_pre);
  125.             X_est(3) = 0;
  126.             X_est(6) = 0;    % Assuming perfect vertical aiding
  127.         P_est_KF = (eye(15) - K*H)*P_pre_KF;
  128.         update = 0;
  129.     else
  130.         X_est = X_pre;
  131.             X_est(3) = 0;
  132.             X_est(6) = 0;    % Assuming perfect vertical aiding
  133.         P_est_KF = P_pre_KF;
  134.     end
  135.     X_pre = PHI*X_est;        
  136.     P_pre_KF = PHI*P_est_KF*PHI' + Q;
  137.     
  138.     state_est(:,i) = X_est;
  139.     x_rms_KF(i) = sqrt( P_est_KF(1,1) );
  140.     y_rms_KF(i) = sqrt( P_est_KF(2,2) );
  141.     x_v_rms_KF(i) = sqrt( P_est_KF(4,4) );
  142.     y_v_rms_KF(i) = sqrt( P_est_KF(5,5) );
  143.     
  144.     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  145.     [rm,rp] = radicurv(est_lat(i));  radius_e = sqrt(rm*rp);
  146.     theta(1,1) = -X_est(2)/radius_e;   % converting position error from linear
  147.     theta(2,1) = X_est(1)/radius_e;    % units to angular units
  148.     theta(3,1) = tan(est_lat(i))*theta(2);  % this is for alpha=0 and ENU frame;
  149.     psi = X_est(7:9);
  150.     phi_angle = psi + theta;         % computing total attitude error
  151.     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  152.      % This is the Kalman estimate of the true DCM
  153.     est_DCMbn_KF = C*(eye(3) + skewsymm(phi_angle))*C*est_DCMbn(:,:,i); 
  154.     
  155.     eul_vect = dcm2eulr(est_DCMbn_KF);
  156.     est_roll_KF(i) = eul_vect(1);
  157.     est_pitch_KF(i) = eul_vect(2);
  158.     est_yaw_KF(i) = eul_vect(3);
  159.     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  160.     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  161.       % This is the Kalman estimate of the true DCM
  162.     est_DCMel_KF = (eye(3) + skewsymm(theta))*est_DCMel(:,:,i);
  163.     
  164.     llw_vect = dcm2llw(est_DCMel_KF);
  165.     est_lat_KF(i) = llw_vect(1);
  166.     est_lon_KF(i) = llw_vect(2);
  167.     est_alpha_KF(i) = llw_vect(3);
  168.     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  169.     est_vel_l_KF(i,:) = ( (eye(3) + skewsymm(theta))*(vel_l(i,:)' - X_est(4:6)) )';
  170.     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  171.     waitbar(i/npts,h)
  172. end
  173. close(h)
  174. h = waitbar(0,' Computing Position, Velocity, Attitude Errors ');
  175. N = max(size(est_lat0));                           % Compute horizontal position
  176. for i = 1:N,                                       % error by finding the ENU
  177.    truxyz = llh2xyz([est_lat0(i) est_lon0(i) 0]);  % coordinates of the 
  178.    insxyz_unaided = llh2xyz([est_lat(i) est_lon(i) 0]);    % INS-derived position 
  179.    enu = xyz2enu(insxyz_unaided,truxyz);                   % relative to the truth
  180.    east_pos_err_unaided(i) = enu(1);
  181.    north_pos_err_unaided(i) = enu(2);
  182.    up_pos_err_unaided(i) = enu(3);
  183.    horz_pos_err_unaided(i) = norm(enu(1:2));
  184.    insxyz_KF = llh2xyz([est_lat_KF(i) est_lon_KF(i) 0]);
  185.    enu = xyz2enu(insxyz_KF,truxyz);
  186.    east_pos_err_KF(i) = enu(1);
  187.    north_pos_err_KF(i) = enu(2);
  188.    up_pos_err_KF(i) = enu(3);
  189.    horz_pos_err_KF(i) = norm(enu(1:2));
  190.    waitbar(i/N,h)
  191. end
  192. close(h)
  193. lat_err_unaided = est_lat-est_lat0;
  194. lon_err_unaided = est_lon-est_lon0;
  195. alpha_err_unaided = est_alpha - est_alpha0;
  196. vel_l_err_unaided = vel_l - vel_l_0;
  197. roll_err_unaided = est_roll - est_roll0;
  198. pitch_err_unaided = est_pitch - est_pitch0;
  199. yaw_err_unaided = est_yaw - est_yaw0;
  200. lat_err_KF = est_lat_KF-est_lat0;
  201. lon_err_KF = est_lon_KF-est_lon0;
  202. alpha_err_KF = est_alpha_KF - est_alpha0;
  203. vel_l_err_KF = est_vel_l_KF - vel_l_0;
  204. roll_err_KF = est_roll_KF - est_roll0;
  205. pitch_err_KF = est_pitch_KF - est_pitch0;
  206. yaw_err_KF = est_yaw_KF - est_yaw0;
  207. close all
  208. subplot(111)
  209. plot(time/60,lat_err_unaided*180/pi,time/60,lon_err_unaided*180/pi,...
  210.      time/60,lat_err_KF*180/pi,time/60,lon_err_KF*180/pi)
  211. title('15-State Filter With Position Aiding')
  212. ylabel('error in degrees')
  213. xlabel('time in minutes')
  214. legend('lat err unaided', 'long err unaided', 'lat err KF', 'lon err KF',0)
  215. figure
  216. subplot(111)
  217. plot(time/60,vel_l_err_unaided(:,1:2),time/60,vel_l_err_KF(:,1:2))
  218. ylabel('velocity error in m/s')
  219. xlabel('time in minutes')
  220. legend('east unaided','north unaided', 'east KF', 'north KF',0)
  221. figure
  222. subplot(111)
  223. plot(time/60,roll_err_unaided*180/pi,time/60,pitch_err_unaided*180/pi,...
  224.      time/60,roll_err_KF*180/pi,time/60,pitch_err_KF*180/pi)
  225. title('Euler Angle Errors')
  226. ylabel('error in degrees')
  227. xlabel('time in minutes')
  228. legend('roll unaided','pitch unaided', 'roll KF', 'pitch KF',0)
  229. figure
  230. subplot(111)
  231. plot(time/60,yaw_err_unaided*180/pi,time/60,yaw_err_KF*180/pi)
  232. ylabel('yaw error in degrees')
  233. xlabel('time in minutes')
  234. legend('yaw unaided', 'yaw KF',0)
  235. figure
  236. plot(time/60,horz_pos_err_unaided,time/60,horz_pos_err_KF)
  237. title('Horizontal Position Error')
  238. ylabel('error in meters')
  239. xlabel('time in minutes')
  240. legend('unaided','KF',0)
  241. figure
  242. plot(time/60,horz_pos_err_KF)
  243. title('Kalman Filter Horizontal Position Error')
  244. ylabel('error in meters')
  245. xlabel('time in minutes')
  246. figure
  247. plot(time/60,x_rms_KF,time/60,y_rms_KF)
  248. title('Position Error Covariance Analysis')
  249. ylabel('rms error in meters')
  250. xlabel('time in minutes')
  251. legend('x','y',0)
  252. figure
  253. plot(time/60,x_v_rms_KF,time/60,y_v_rms_KF)
  254. title('Velocity Error Covariance Analysis')
  255. ylabel('rms error in meters/sec')
  256. xlabel('time in minutes')
  257. legend('x','y',0)