初始对准实验(1)_KF_初始对准_stateestimation_对准_SINS初始对准_

%% SINS 静态基座初始对准 clc clear all close all %% 导入数据 data=load('imu_data-2013年03月26日20时04分55秒.dat'); % load track.dat; j=1;k=1; %% 读取数据 % for i = 1:length(data) % a(i,:)=data(i,6:8); % gyro(i,:)=data(i,3:5); % tread(i,1)=data(i,2); % end Row = length(data)*2; %% 设置参数 % ……常量参数…………………………… wie=0.000072921151467; %地球自转角速度 re=6378135.072; %地球半径 e=1/298.25; %地球偏心率 rad=pi/180; %弧度转换 g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013; %地球重力加速度 ug=1e-6*g0; %定义 ug T=0.01; %滤波和采样周期/s % ……设定初值…………………………… lat0=39.97912*rad; %初始纬度（弧度） lon0=116.34681*rad; %初始经度（弧度） hei0=100.38; %初始高度（米） fai(1)= -285*rad; %初始航向角 theta(1)=0*rad; %初始俯仰角 gamma(1)=0*rad; %初始横滚角 vx=0; %初始东向水平速度(m/s) vy=0; %初始北向水平速度(m/s) vz=0; %初始天向速度(m/s) lat=zeros(1,Row/2+1); %用于记录实时纬度 lon=zeros(1,Row/2+1); %用于记录实时经度 hei = zeros(1,Row/2+1); %用于记录实时高度 lat(1)=lat0/rad; %将初始纬度加入到行阵 lat 中 lon(1)=lon0/rad; %将初始经度加入到行阵 lon 中 hei(1)=hei0; %将初始高度加入到行阵 hei 中 %g=g0*(1+gk1*sin(lat(1))^2)/sqrt(1-gk2*sin(lat(1))^2)*(1-2*hei(1)/re); W=zeros(3,Row/2); %用于记录实时三轴指令角速度 V=zeros(3,Row/2); %用于记录实时东北天速度 attitude=zeros(3,Row/2); %用于记录姿态角 X = zeros(10,Row/2);%用于存储状态 Uusv = []; Susv = []; Vusv = []; S = []; % ……仿真条件…………………………… gyroc=0.01*rad/3600; %陀螺常值漂移 0.01°/h gyros=0.01*rad/3600; %陀螺随机漂移 0.01°/h accec=50*ug; %加计常值偏置 50ug acces=50*ug; %加计随机偏置 50ug ori_szj=0.05*rad; %初始失准角 0.05° %% 读取数据 for i = 1:length(data) a(i,:)=data(i,6:8)*g0; gyro(i,:)=data(i,3:5)*rad/3600; tread(i,1)=data(i,2); end ins_gyro=gyro'; %加噪声后的陀螺 3x200001 ins_acce=-a'; %加噪声后的加计 3x200001 % ins_gyro = T * [ gyro(:,1)' / T + gyroc + gyros^2 * randn(1, Row/2); % gyro(:,2)' / T + gyroc + gyros^2 * randn(1, Row/2); % gyro(:,3)' / T + gyroc + gyros^2 * randn(1, Row/2)]; % ins_acce = T * [ a(:,1)'/ T + accec + acces^2 * randn(1, Row/2); % a(:,2)'/ T + accec + acces^2 * randn(1, Row/2); % a(:,3)'/ T + accec + acces^2 * randn(1, Row/2)]; % ……滤波参数…………………………… H=[eye(2,2),zeros(2,8)]; %量测矩阵 H P0=diag([(0.1)^2,(0.1)^2,ori_szj^2,ori_szj^2,ori_szj^2,accec^2,accec^2,gyroc^2,gyroc^2,gyroc^2]); %P0 阵 Q=diag([acces^2,acces^2,gyros^2,gyros^2,gyros^2,0,0,0,0,0]); %Q 阵 R=diag([(0.1)^2,(0.1)^2]); %R 阵 X0=[0 0 ori_szj ori_szj ori_szj 0 0 0 0 0 ]'; %X0 Xk=X0; %% ……粗对准 正交向量法1…………………………… acce0=[0;0;0]; gyro0=[0;0;0]; %%粗对准 正交向量法1 Tnb0 = T_N_B(fai(1),theta(1),gamma(1)); for i = 1:30000 acce0=acce0+ins_acce(:,i); gyro0=gyro0+ins_gyro(:,i); end acce0=acce0/30000; gyro0=gyro0/30000; Vn_T_inv1=[0,0,-g0/g0]; Vn_T_inv2=[0,wie*cos(lat(1)),wie*sin(lat(1))]/wie; Vn_T_inv3=cross(Vn_T_inv1,Vn_T_inv2)/norm(cross(Vn_T_inv1,Vn_T_inv2)); Vn_T_inv4=cross(Vn_T_inv3,Vn_T_inv1)/norm(cross(Vn_T_inv3,Vn_T_inv1)); Vn_T_inv =[Vn_T_inv1;Vn_T_inv3;Vn_T_inv4]; Vb_T1 = [acce0(1),acce0(2),acce0(3)]; Vb_T2 = [gyro0(1) gyro0(2) gyro0(3)]; Vb_T3 = cross(Vb_T1/norm(Vb_T1),Vb_T2/norm(Vb_T2))/norm(cross(Vb_T1/norm(Vb_T1),Vb_T2/norm(Vb_T2))); Vb_T4 = cross(Vb_T3,Vb_T1/norm(Vb_T1))/norm(cross(Vb_T3,Vb_T1/norm(Vb_T1))); Vb_T = [Vb_T1;Vb_T3;Vb_T4]; Tnb1 = Vn_T_inv^(-1)*Vb_T; % ……一次修正粗对准……………… acce0 = Tnb1*acce0; gyro0 = Tnb1*gyro0; phi = [acce0(2)/g0;-acce0(1)/g0;gyro0(1)/wie*cos(lat(1)) - acce0(1)*tan(lat(1))/g0]; Tnb = (eye(3,3) + OmegaMatrix(phi))*Tnb0; Tnb0 = Tnb; T_12 = Tnb(1,2); %求姿态角 T_22 = Tnb(2,2); T_31 = Tnb(3,1); T_32 = Tnb(3,2); T_33 = Tnb(3,3); fai = atan((-T_12)/T_22); if(abs(Tnb(2,2))<1e-8) %处理航向角多值问题 if(Tnb(2,1)>0) fai=-pi/2; else fai=pi/2; end else if(Tnb(2,2)>0) if(fai<0) fai=fai+2*pi; end else fai=fai+pi; end end theta = atan(T_32/sqrt(T_12^2 + T_22^2)); gamma = atan((-T_31)/T_33); if(T_33 < 0) %处理横滚角多值问题 if(gamma> 0) gamma = gamma - pi; else gamma = gamma + pi; end end %% ……精对准………………… % 初始四元数 q=zeros(4,1); q(1)=cos(fai/2)*cos(theta/2)*cos(gamma/2)-sin(fai/2)*sin(theta/2)*sin(gamma/2); q(2)=cos(fai/2)*sin(theta/2)*cos(gamma/2)-sin(fai/2)*cos(theta/2)*sin(gamma/2); q(3)=cos(fai/2)*cos(theta/2)*sin(gamma/2)+sin(fai/2)*sin(theta/2)*cos(gamma/2); q(4)=cos(fai/2)*sin(theta/2)*sin(gamma/2)+sin(fai/2)*cos(theta/2)*cos(gamma/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;]; for t=1:Row/2 rx=re/(1-e*sin(lat0)*sin(lat0))+hei0; ry=re/(1+2*e-3*e*sin(lat0)*sin(lat0))+hei0; g=g0*(1+gk1*sin(lat0)^2)/sqrt(1-gk2*sin(lat0)^2); wien=[0;wie*cos(lat0);wie*sin(lat0)]; wenn=[-vy/ry;vx/rx;vx/rx*tan(lat0)]; W(:,t)=wien+wenn; %实时三轴指令角速度 witb=Cnb'*W(:,t); %指令角速度转换到载体系 % ……姿态更新…………… w=ins_gyro(:,t)-witb; %载体系相对于地理系的角速度 ang=w*T; %解四元数微分方程 Dsta=[ 0 -ang(1) -ang(2) -ang(3); ang(1) 0 ang(3) -ang(2); ang(2) -ang(3) 0 ang(1); ang(3) ang(2) -ang(1) 0]; ang0=norm(ang); q=((1-ang0^2/8-ang0^4/348)*eye(4,4)+(.5-ang0^2/48)*Dsta)*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];%四元数姿态矩阵 attitude(1,t)=asin(Cnb(2,3)); %theta attitude(2,t)=atan(-Cnb(1,3)/Cnb(3,3)); %gamma attitude(3,t)=atan(-Cnb(2,1)/Cnb(2,2)); %fai if(abs(Cnb(2,2))<1e-8) %处理航向角多值问题 if(Cnb(2,1)>0) attitude(3,t)=-pi/2; else attitude(3,t)=pi/2; end else if(Cnb(2,2)>0) if(attitude(3,t)<0) attitude(3,t)=attitude(3,t)+2*pi; end else attitude(3,t)=attitude(3,t)+pi; end end if(abs(Cnb(3,3))<1e-8) % 处理横滚角多值问题 if(Cnb(1,3)>0) attitude(2,t)=-pi/2; else attitude(2,t)=pi/2; end else if(Cnb(3,3)<0) if(attitude(2,t)>0) attitude(2,t)=attitude(2,t)-pi; else attitude(2,t)=attitude(2,t)+pi; end end end attitude(:,t)=[attitude(1,t);attitude(2,t);attitude(3,t)]; q = [cos(attitude(3,t)/2)*cos(attitude(1,t)/2)*cos(attitude(2,t)/2) - sin(attitude(3,t)/2)*sin(attitude(1,t)/2)*sin(attitude(2,t)/2); cos(attitude(3,t)/2)*sin(attitude(1,t)/2)*cos(attitude(2,t)/2) - sin(attitude(3,t)/2)*cos(attitude(1,t)/2)*sin(attitude(2,t)/2); cos(attitude(3,t)/2)*cos(attitude(1,t)/2)*sin(attitude(2,t)/2) + sin(attitude(3,t)/2)*sin(attitude(1,t)/2)*cos(attitude(2,t)/2); cos(attitude(3,t)/2)*sin(attitude(1,t)/2)*sin(attitude(2,t)/2) + sin(attitude(3,t)/2)*cos(attitude(1,t)/2)*cos(attitude(2,t)/2)]; % ……速度更新…………… eacce = (2.0*wien(3)+wenn(3))*vy - (2.0*wien(2)+wenn(2))*vz; %东向加速度更新 nacce = -(2.0*wien(3)+wenn(3))*vx + (2.0*wien(1)+wenn(1))*vz; %北向加速度更新 uacce = (2.0*wien(2)+wenn(2))*vx - (2.0*wien(1)+wenn(1))*vy - g; %天向加速度更新 ft=Cnb*ins_acce(:,t); %加计在地理系上的分量 vx=vx+(ft(1)+eacce)*T; %东向速度更新 vy=vy+(ft(2)+nacce)*T; %北向速度更新 vz=vz+(ft(3)+uacce)*T; %天向速度更新 V(:,t)=[vx,vy,vz]'; %实时速度记录 % ……位置更新…………… lat0=lat0+vy*T/ry; %地理纬度更新 lon0=lon0+vx*T/rx/cos(lat0); %地理经度更新 hei0=hei0+vz*T ;%高度更新 lat(t+1)=lat0/rad; %实时纬度记录 lon(t+1)=lon0/rad; %实时经度记录 hei(t+1)=hei0;%实时高度记录 % ……定义卡尔曼滤波参数…………… Cbn=Cnb'; Z=[vx;vy]; %量测量 A=zeros(10,10); %�