卡尔曼滤波组合导航程序_编写基于kalman滤波器的sins/gps间接法组合导航程序,卡尔曼滤波组合导航matlab

clear all; close all; clc; load('E:\实验室资料\学习资料\研一下学习资料\卡尔曼滤波与组合导航\kalman实验报告__邹莹\实验三\数据\IMU.dat') load('E:\实验室资料\学习资料\研一下学习资料\卡尔曼滤波与组合导航\kalman实验报告__邹莹\实验三\数据\GPS.dat') % Pi = 3.1415926535897931; % WIEE = 0.000072921151467; % Re = 6378135.072; % g0 = 9.7803267714; % e = 1.0 / 298.25; % T_IMU = 0.01; %%导航数据时间间隔 % T_GPS = 0.05; %%GPS数据时间间隔 % LOOP = 360000; %%循环次数，数据长度 % % % velocity_GPSINS(7200,3) = 0; % position_GPSINS(7200,3) = 0; % attitude(LOOP,3) = 0; % x_kf(LOOP/5,15) = 0; % p_kf(LOOP/5,15) = 0; % % %导航参数初始化赋值 % Lat = GPS(1,3)*Pi/180; % Lon = GPS(1,4)*Pi/180; % Hei = GPS(1,5); % Vx = GPS(1,6); % Vy = GPS(1,7); % Vz = GPS(1,8); % % IMU(1:360001,3:5) = IMU(1:360001,3:5)*Pi/180/3600; % IMU(1:360001,6:8) = IMU(1:360001,6:8)*g0; % %姿态角 % fai = 305.34023*Pi/180; % theta = 0.25097*Pi/180; % gama = 1.78357*Pi/180; % % %初始方向余弦矩阵和四元数 % 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(fai), 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/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)]; % % Kf_loop = 0; % Q = diag([(0.2*Pi/180/3600)^2,(0.2*Pi/180/3600)^2,(0.2*Pi/180/3600)^2,(100E-6*g0)^2,(100E-6*g0)^2,(100E-6*g0)^2]); % R = diag([0.01^2,0.01^2,0.01^2,0.1^2,0.1^2,0.15^2]); % p_filter = diag([(0.5*Pi/180)^2,(1*Pi/180/60)^2,(1*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,(50E-6*g0)^2,(50E-6*g0)^2,(50E-6*g0)^2]); % x_filter = zeros(15,1); % % for i= 1:360000 % Rmh = Re * (1 - 2 * e + 3 * e * sin(Lat) * sin(Lat)) + Hei; % Rnh = Re * (1 + e * sin(Lat) * sin(Lat)) + Hei; % g_s = g0 * (1 + 0.00193185138639 * sin(Lat) * sin(Lat)) / sqrt(1 - 0.00669437999013 * sin(Lat) * sin(Lat)) * (1 - 2 * Hei / Re); % Wien = [ 0; WIEE * cos(Lat); WIEE * sin(Lat)]; % Wenn = [ -Vy / Rmh; Vx / Rnh; Vx * tan(Lat) / Rnh]; % Winn = Wien + Wenn; % Winb = Cnb * Winn; % Wibb = IMU(i,3:5)'; % Wnbb = Wibb - Winb; % angle = Wnbb * T_IMU; %%每次姿态变化对应的小角度变化，用于姿态更新 % % % % edit by STX % % fn = Cnb' * IMU(i,6:8)'; %%比力在n系中的投影 % % V=[Vx;Vy;Vz]; % % FN=[fn(1);fn(2);fn(3)]; % % Wx_temp=2*Wien+Wenn; % % g=[0;0;g_s]; % % difV=FN-cross(Wx_temp,V)-g; % % difVx=difV(1); % % difVy=difV(2); % % difVz=difV(3); % % Vx=V(1);Vy=V(2);Vz=V(3); % % % fn = Cnb' * IMU(i,6:8)'; %%比力在n系中的投影 % difVx = fn(1) + (2 * WIEE * sin(Lat) + Vx * tan(Lat) / Rnh) * Vy - (2 * WIEE * cos(Lat) + Vx / Rnh) * Vz; % difVy = fn(2) - (2 * WIEE * sin(Lat) + Vx * tan(Lat) / Rnh) * Vx + Vy * Vz / Rmh; % difVz = fn(3) + (2 * WIEE * cos(Lat) + Vx / Rnh) * Vx + Vy * Vy /Rmh - g_s; %%P87 4.2.42 速度微分方程，比力方程 % % % Vx = difVx * T_IMU + Vx; % Vy = difVy * T_IMU + Vy; % Vz = difVz * T_IMU + Vz; % % Lat = Lat + Vy * T_IMU / Rmh; % Lon = Lon + Vx * T_IMU / Rnh / cos(Lat); % Hei = Hei + Vz * T_IMU; % % % 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(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)]; % % fai = atan(-Cnb(2,1) / Cnb(2,2)); % theta = asin(Cnb(2,3)); % gama = atan(-Cnb(1,3) / Cnb(3,3)); % % if (Cnb(2,2) < 0) % fai = fai + Pi; % elseif (fai < 0) % fai = fai + 2*Pi; % end % if (Cnb(3,3) < 0) % if(gama > 0) % gama = gama - Pi; % else % gama = gama + Pi; % end % end % % attitude(i,:) = [fai/Pi*180, theta/Pi*180, gama/Pi*180]; % %滤波开始 % if (mod(i,5) == 0) % % Kf_loop = Kf_loop + 1; %%GPS数据频率是IMU的五分之一，所以kalman滤波的频率为1/5 % % % Rmh = Re * (1 - 2 * e + 3 * e * sin(Lat) * sin(Lat)) + Hei; % % Rnh = Re * (1 + e * sin(Lat) * sin(Lat)) + Hei; % % Hv = [zeros(3,3),eye(3,3),zeros(3,9)]; % Hp = [zeros(3,6),diag([Rmh,Rnh*cos(Lat),1]),zeros(3,6)]; % H = [Hv;Hp]; % % G = [Cnb',zeros(3,3);zeros(3,3),Cnb';zeros(9,3),zeros(9,3)]; %噪声驱动阵 % % FN = zeros(9,9); % FN(1,2) = WIEE * sin(Lat) + Vx / Rnh * tan(Lat); % FN(1,3) = -WIEE * cos(Lat) - Vx / Rnh; % FN(1,5) = -1.0 / Rmh; % FN(1,9) = 1.0 * Vy / Rmh / Rmh; % FN(2,1) = -WIEE * sin(Lat) - Vx / Rnh * tan(Lat); % FN(2,3) = -Vy / Rmh; % FN(2,4) = 1.0 / Rnh; % FN(2,7) = -WIEE * sin(Lat); % FN(2,9) = -Vx / Rnh / Rnh; % FN(3,1) = WIEE * cos(Lat) + Vx / Rnh; % FN(3,2) = Vy / Rmh; % FN(3,4) = 1.0 / Rnh * tan(Lat); % FN(3,7) = WIEE * cos(Lat) + Vx / Rnh / cos(Lat) / cos(Lat); % FN(3,9) = -Vx / Rnh / Rnh * tan(Lat); % FN(4,2) = -fn(3); % FN(4,3) = fn(2); % FN(4,4) = 1.0 / Rnh * (Vy * tan(Lat) - Vz); % FN(4,5) = 2.0 * WIEE * sin(Lat) + Vx / Rnh * tan(Lat); % FN(4,6) = -2.0 * WIEE * cos(Lat) - Vx / Rnh; % FN(4,7) = 2.0 * WIEE * cos(Lat) * Vy + 2.0 * WIEE * sin(Lat) * Vz + Vx * Vy / Rnh / cos(Lat) / cos(Lat); % FN(4,9) = 1* (Vx * Vz - Vx * Vy * tan(Lat)) / Rnh / Rnh; % FN(5,1) = fn(3); % FN(5,3) = -fn(1); % FN(5,4) = -2.0 * WIEE * sin(Lat) - 2.0 * Vx / Rnh * tan(Lat); % FN(5,5) = -Vz / Rmh; % FN(5,6) = -Vy / Rmh; % FN(5,7) = -Vx * (2.0 * WIEE * cos(Lat) + Vx / Rnh / cos(Lat) / cos(Lat)); % FN(5,9) = (Vy * Vz + Vx * Vx * tan(Lat)) / Rnh / Rnh; % FN(6,1) = -fn(2); % FN(6,2) = fn(1); % FN(6,4) = 2.0 * WIEE * cos(Lat) + 2.0 * Vx / Rnh; % FN(6,5) = 2.0 * Vy / Rmh; % FN(6,7) = -2.0 * WIEE * sin(Lat) * Vx; % FN(6,9) = -(Vx * Vx + Vy * Vy) / Rnh / Rnh; % FN(7,5) = 1.0 / Rmh; % FN(7,9) = -Vy / Rmh / Rmh; % FN(8,4) = 1.0 / Rnh / cos(Lat); % FN(8,7) = tan(Lat) / Rnh / cos(Lat) * Vx; % FN(8,9) = -Vx / Rnh / Rnh / cos(Lat); % FN(9,6) = 1; % FS = [Cnb',zeros(3,3);zeros(3,3),Cnb';zeros(3,3),zeros(3,3)]; % F = [FN,FS;zeros(6,9),zeros(6,6)]; % % F = F * T_GPS; % temp1 = eye(15); % disA = eye(15); % for j = 1:10 % temp1 = F * temp1 / j; % disA = disA + temp1; % Faikk-1 % end % % GQG = G * Q * G'; % temp1 = GQG * T_GPS; % disB = temp1; % for j = 2:11 % temp2 = F * temp1; % temp1 = (temp2 + temp2')/j; % disB = disB + temp1; %%噪声阵为什么也这样按FAI阵累加 % end % % z = [Vx-GPS(Kf_loop,6);Vy-GPS(Kf_loop,7);Vz-GPS(Kf_loop,8);