jing_alignmet2.m

惯性导航精解算程序

function [ j_theta j_gama j_fai ] = jing_alignmet2( theta0,gama0,fai0,theta,gama,fai,latitude,longitude )
%%捷联惯导系统静基座对准的卡尔曼滤波程序，观测量为速度误差,自己写的啊激光陀螺寻北系统建模与参数辨识,捷联式惯导系统初始对准方法研究
LL0 = latitude;%初始的纬度

LO0 =longitude;%初始的经度

g =  9.7803267714*(1+0.00193185138639*sin(LL0(1))^2)/sqrt(1-0.00669437999013*sin(LL0(1))^2);
Ra = 6378140;%地球参考椭球长轴半径
e =  1/298.257;%0%地球椭球的椭圆度 要和g一起修改那
wie = 15.04088/3600/180*pi; %0;%%地球自转角速度
wie_n=wie*cos(LL0);  %导航坐标系为当地地理坐标系
wie_u=wie*sin(LL0);
T_align = 3*60;%对准时间为5分钟
T_samp = 0.01;%惯性器件的采样时间
number_data  = T_align/T_samp;%数据个数为30000个
%% 设定初始捷联矩阵
% fai = 0;theta = 0;gama=0;
%calculate of ideal strapdown_matrix from chenzhe
Cb_n = [sin(fai)*sin(theta)*sin(gama)+cos(fai)*cos(gama)   cos(fai)*sin(theta)*sin(gama)-sin(fai)*cos(gama)   -cos(theta)*sin(gama);
        sin(fai)*cos(theta)                                cos(fai)*cos(theta)              sin(theta);
        cos(fai)*sin(gama)-sin(fai)*sin(theta)*cos(gama)   -sin(fai)*sin(gama)-cos(fai)*sin(theta)*cos(gama)    cos(theta)*cos(gama) ];
Cn_b=Cb_n';
%% 初始值的设定
% 导航坐标系为东北天
% fiee = 1*60*60;%初始误差角为1度，单位为角秒 1度=3600角秒
% fien = 1*60*60;
% fieu = 1*60*60;
%% 观测量的生成

tra(T_samp,T_align,LL0,LO0,theta0,gama0,fai0);
acc_gy(1,T_samp,T_align);%陀螺仪加速度的输出,带噪声
% fpin = fopen('guanceliang.dat','r');% 打开文件啊，观测量速度误差

jiesuan(theta,gama,fai);%生成解算后的数据
load imusolve_silent;
%% 东北天坐标系下系统的状态矩阵（这里是连续系统）
A=zeros(10,10);                                                  
A(1,2)=2*wie_u;
A(1,4)=-g;
A(1,6)=C_nb(1,1);
A(1,7)=C_nb(1,2);
A(2,1)=-2*wie_u;
A(2,3)=g;
A(2,6)=C_nb(2,1);
A(2,7)=C_nb(2,2);
A(3,2)=-1/Ra;%自己添上的啊
A(3,4)=wie_u;
A(3,5)=-wie_n;
A(3,8)=C_nb(1,1);
A(3,9)=C_nb(1,2);
A(3,10)=C_nb(1,3);
A(4,1)=1/Ra;%自己添上的啊
A(4,3)=-wie_u;
A(4,8)=C_nb(2,1);
A(4,9)=C_nb(2,2);
A(4,10)=C_nb(2,3);
A(5,1)=tan(LL0)/Ra;%自己添上的啊
A(5,3)=wie_n;
A(5,8)=C_nb(3,1);
A(5,9)=C_nb(3,2);
A(5,10)=C_nb(3,3);
%观测矩阵
H=zeros(2,10);                                                   
H(1,1)=1;
H(2,2)=1;
%初始状态向量  初始状态都设为0 初始状态分别代表速度误差2，姿态误差角3，加速度测量误差2，角速度测量误差3
X=zeros(10,1);                                                   
X(3,1)=theta;
X(4,1)=gama;
X(5,1)=fai;
%初始观测值   初始观测值设为0
Z=zeros(2,1);
%初始方差阵的设置
%初始速度误差取0.1米每妙 初始失准角均为1度 
%加速度计的初始偏值均取1e-4*g 陀螺的常值漂移取0.02度每小时
P=zeros(10,10);
P(1,1)=0.1^2;%水平速度的误差
P(2,2)=0.1^2;
P(3,3)=(1*pi/180)^2;%姿态误差角，1度，转化为弧度
P(4,4)=(1*pi/180)^2;
P(5,5)=(1*pi/180)^2;
P(6,6)=(1e-4*g)^2;%加速度初始偏值
P(7,7)=(1e-4*g)^2;

P(8,8)=(2*pi/180/3600)^2;%陀螺常值偏移，转化为弧度/秒
P(9,9)=(2*pi/180/3600)^2;
P(10,10)=(2*pi/180/3600)^2;
%＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝初始系统噪声误差阵＝＝＝＝＝＝＝＝＝＝＝＝＝＝%
%加速度计的随机偏差为0.5e-4*g  陀螺的随机漂移为1度每小时
Q=zeros(10,10);                                                 
Q(1,1)=(5e-5*g)^2;
Q(2,2)=(5e-5*g)^2;
Q(3,3)=(1*pi/180/3600)^2;
Q(4,4)=(1*pi/180/3600)^2;
Q(5,5)=(1*pi/180/3600)^2;
%初始观测噪声误差阵
R=zeros(2,2);                                                   
R(1,1)=0.1^2;%0.1米/秒
R(2,2)=0.1^2;
%==================状态矩阵和系统噪声阵的离散化====================% 
T_discre=0.01;                                              %离散化时间
FI=eye(10);
Qdct=zeros(10);
        temp2=1;
        M{1}=Q;
        for j=1:10
            temp2=temp2*j;
            FI=FI+T_discre^j*(A)^j/temp2;
            if(j~=1)
                M{j}=A*M{j-1}+(A*M{j-1})';
            end
            Qdct =Qdct+T_discre^j*M{j}/temp2;
        end
        
for i=1:number_data;          %kalman滤波开始
         
%         fscanf(fpin, '%g', 1);
%         m=fscanf(fpin, '%g', 1);
%         n=fscanf(fpin, '%g', 1);
        m = Storedata_solve(1+i,4);%vx
        n = Storedata_solve(1+i,5);
        Z(1,1)=m;
        Z(2,1)=n;
        
        
        
%         a=fscanf(fpin, '%g', 1);                        %单位：角秒    
%         b=fscanf(fpin, '%g', 1);
%         c=fscanf(fpin, '%g', 1);   
a=Storedata_solve(1+i,10);% 单位：弧度
b=Storedata_solve(1+i,11);
c=Storedata_solve(1+i,12);
 
%============================== mine new2 ===============================%

%============================== new2 end ================================%


     P=FI*P*FI'+Qdct;
     K=P*H'*inv(H*P*H'+R);
     X=FI*X+K*(Z-H*FI*X);
     P=(eye(10)-K*H)*P*(eye(10)-K*H)'+K*R*K';
     
     %估计误差的方差      
     ddeltav_e(i)=sqrt(P(1,1));                 %速度估计误差 单位 m/s           
     ddeltav_n(i)=sqrt(P(2,2));              
     dfiee(i)=sqrt(P(3,3))*180*60/pi;           %姿态误差角估计误差 单位 角分
     dfien(i)=sqrt(P(4,4))*180*60/pi;
     dfieu(i)=sqrt(P(5,5))*180*60/pi;
     daccle_meter_e(i)=sqrt(P(6,6));%*1e+6/g;     %加速度计偏置 单位 ug 
     daccle_meter_n(i)=sqrt(P(7,7));%*1e+6/g;%标准单位啊
     dgyro_e(i)=sqrt(P(8,8))*180/pi*3600;%/60/60*3600;       %陀螺漂移  单位度每小时
     dgyro_n(i)=sqrt(P(9,9))*180/pi*3600;%/60/60*3600;
     dgyro_u(i)=sqrt(P(10,10))*180/pi*3600;%/60/60*3600;
     dfiee(i)=a-(X(3,1));%/60;  弧度                                  %保存三个姿态误差角
     dfien(i)=b-(X(4,1));%/60;
     dfieu(i)=c-(X(5,1));%/60;

     
end
%----------------------------------- 画图----------------------------------
t=0.01:0.01:300/2;
figure(1)
plot(ddeltav_e);
title('aaa');
figure(2)
plot(ddeltav_n)
title('北向速度估计误差');
figure(3)
subplot(3,1,1)
plot(dfiee)
subplot(3,1,2)
plot(dfien)
subplot(3,1,3)
plot(dfieu)
title('姿态角估计误差');
figure(4)
plot(daccle_meter_e)
title('东向加速度估计误差');
figure(5)
plot(daccle_meter_n)
title('北向速度估计误差');
figure(6)
subplot(3,1,1)
plot(dgyro_e)
subplot(3,1,2)
plot(dgyro_n)
subplot(3,1,3)
plot(dgyro_u)
title('陀螺仪估计误差');