kf_sins_modify2.m

这是一个用MATLAB编写的接联惯性导航系统精对准阶段KALMAN滤波仿真程序的修正程序。解决了因系数矩阵条件数过大造成的滤波发散问题。

%============本程序为捷联惯导系统静基座对准卡尔曼滤波程序的修正程序=====
%==========================观测量速度误差===========================
%==========================================================================
clear all
close all
clc
%------------------------------常量定义区----------------------------------
L=45*pi/180;     %初始纬度
g0=9.78049;
g=g0*(1+0.0052884*sin(L)*sin(L)-0.0000059*sin(2*L)*sin(2*L));    %实验当地重力加速度计算
wie=0.00007272205216643039903848711535368;	        %地球自转角速度  单位：弧度每秒
wie_e=0;
wie_n=wie*cos(L);  %导航坐标系为当地地理坐标系
wie_u=wie*sin(L);

Re=6378137.0;	       %地球半径:1984年WGS-84全球
T_align=5*60;          %对准时间为5分钟
T_samp=0.01;           %惯性器件的采样时间
number_data=T_align/T_samp;        %数据个数 30000个
H_JM=60*60*180/pi;     %角秒到弧度
JM_H=pi/180/60/60;     %弧度到角秒
%--------------------------设定初始捷联矩阵-----------------------------------
%姿态角设定
yaw_angle=pi*45/180;          %偏航角
pitching_angle=pi*30/180;     %俯仰角
turn_angle=pi*5/180;          %滚动角
%calculate of ideal strapdown_matrix from chenzhe
Cbt_idea11=cos(turn_angle)*cos(yaw_angle)-sin(turn_angle)*sin(pitching_angle)*sin(yaw_angle);
Cbt_idea12=cos(turn_angle)*sin(yaw_angle)+sin(turn_angle)*sin(pitching_angle)*cos(yaw_angle);
Cbt_idea13=-sin(turn_angle)*cos(pitching_angle);
Cbt_idea21=-cos(pitching_angle)*sin(yaw_angle);
Cbt_idea22=cos(pitching_angle)*cos(yaw_angle);
Cbt_idea23=sin(pitching_angle);
Cbt_idea31=sin(turn_angle)*cos(yaw_angle)+cos(turn_angle)*sin(pitching_angle)*sin(yaw_angle);
Cbt_idea32=sin(turn_angle)*sin(yaw_angle)-cos(turn_angle)*sin(pitching_angle)*cos(yaw_angle);
Cbt_idea33=cos(turn_angle)*cos(pitching_angle);
Cbt_idea=[Cbt_idea11 Cbt_idea12 Cbt_idea13
          Cbt_idea21 Cbt_idea22 Cbt_idea23
          Cbt_idea31 Cbt_idea32 Cbt_idea33];
Ctb_idea=Cbt_idea';
%------------------------------初始值的设定--------------------------------
%导航坐标系选东北天
fiee=1*60*60;        %初始误差角均为1度  单位角秒
fien=1*60*60;
fieu=1*60*60;
fpin=fopen('guanceliang3.dat', 'r');          %打开文件，观测量数据（人造）
%东北天坐标系下系统的状态矩阵(连续系统）
A=zeros(10,10);                                                  
A(1,2)=2*wie_u;
A(1,4)=-g*JM_H;
A(1,6)=Ctb_idea(1,1);
A(1,7)=Ctb_idea(1,2);
A(2,1)=-2*wie_u;
A(2,3)=g*JM_H;
A(2,6)=Ctb_idea(2,1);
A(2,7)=Ctb_idea(2,2);
A(3,4)=wie_u;
A(3,5)=-wie_n;
A(3,8)=Ctb_idea(1,1);
A(3,9)=Ctb_idea(1,2);
A(3,10)=Ctb_idea(1,3);
A(4,3)=-wie_u;
A(4,8)=Ctb_idea(2,1);
A(4,9)=Ctb_idea(2,2);
A(4,10)=Ctb_idea(2,3);
A(5,3)=wie_n;
A(5,8)=Ctb_idea(3,1);
A(5,9)=Ctb_idea(3,2);
A(5,10)=Ctb_idea(3,3);
%观测矩阵
H=zeros(2,10);                                                   
H(1,1)=1;
H(2,2)=1;
%初始状态向量  初始状态都设为0
X=zeros(10,1);                                                   
X(3,1)=0*60*60;
X(4,1)=0*60*60;
X(5,1)=0*60*60;
%初始观测值   初始观测值设为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*60*60)^2;
P(4,4)=(1*60*60)^2;
P(5,5)=(1*60*60)^2;
P(6,6)=(1e-4*g)^2;
P(7,7)=(1e-4*g)^2;
P(8,8)=(0.02*60*60/3600)^2;
P(9,9)=(0.02*60*60/3600)^2;
P(10,10)=(0.02*60*60/3600)^2;

%＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝初始系统噪声误差阵＝＝＝＝＝＝＝＝＝＝＝＝＝＝%
%加速度计的随机偏差为0.5e-4*g  陀螺的随机漂移为0.01度每小时
Q=zeros(10,10);                                                 
Q(1,1)=(5e-5*g)^2;
Q(2,2)=(5e-5*g)^2;
Q(3,3)=(0.01*60*60/3600)^2;
Q(4,4)=(0.01*60*60/3600)^2;
Q(5,5)=(0.01*60*60/3600)^2;
%初始观测噪声误差阵
R=zeros(2,2);                                                   
R(1,1)=0.1^2;
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
        
        
%============================== mine new1===============================


%============================ new1 end===================================
        

        
for i=1:number_data/2;          %kalman滤波开始
         
        fscanf(fpin, '%g', 1);
        m=fscanf(fpin, '%g', 1);
        n=fscanf(fpin, '%g', 1);
      
        Z(1,1)=m;
        Z(2,1)=n;
        
        
        
        a=fscanf(fpin, '%g', 1);                        %单位：角秒    
        b=fscanf(fpin, '%g', 1);
        c=fscanf(fpin, '%g', 1);   
 
%============================== 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))/60/60*3600;       %陀螺漂移  单位度每小时
     dgyro_n(i)=sqrt(P(9,9))/60/60*3600;
     dgyro_u(i)=sqrt(P(10,10))/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(t,ddeltav_e)
%figure(2)
%plot(t,ddeltav_n)
figure(3)
subplot(3,1,1)
plot(t,dfiee)
subplot(3,1,2)
plot(t,dfien)
subplot(3,1,3)
plot(t,dfieu)
%figure(4)
%plot(t,daccle_meter_e)
%figure(5)
%plot(t,daccle_meter_n)
%figure(6)
%subplot(3,1,1)
%plot(t,dgyro_e)
%subplot(3,1,2)
%plot(t,dgyro_n)
%subplot(3,1,3)
%plot(t,dgyro_u)