centergps.m

本程序主要设计了一个组合导航的简便的matlab程序的实现

wie=0.792*10^-4;%地球自转角速率
Re=6378245*(1-sin(25/57.2958)^2/298.3);%出发点地球半径，57.2958-度转弧度
wd=25/57.2958;jd=120/57.2958;%wd-纬度，jd-经度
g=9.8;%地球加速度
tg=3600;td=300;tm=3000;th=1000;tgps=100;%陀螺慢变漂移、多普勒、磁航向仪、深度计、GPS相关时间
RK=diag([0.002^2,0.002^2,0.002^2,(0.1/60/57.2958)^2,0.005^2,(0.001/60/57.2958)^2,(0.001/60/57.2958)^2,10^2,0.004^2,0.004^2,0.004^2]);%系统噪声方差阵
Q=diag([(0.1/57.2958/60)^2,(0.1/57.2958/60)^2,(0.1/57.2958/60)^2,(0.1/57.2958/3600)^2/1800,(0.1/57.2958/3600)^2/1800,(0.1/57.2958/3600)^2/1800,(10^-4*g)^2,(10^-4*g)^2,(10^-4*g)^2,0.02^2/150,0.02^2/150,0.02^2/150,(0.1/57.2958)^2/1500,0.05^2/500,(0.02/60/57.2958)^2/50,(0.02/60/57.2958)^2/50,50^2,0.25^2,0.25^2,0.25^2]);%驱动噪声强度阵
PK=diag([0,0,0,0,0,0,0,0,0,(0.1/57.2958/3600)^2,(0.1/57.2958/3600)^2,(0.1/57.2958/3600)^2,(0.1/57.2958/3600)^2,(0.1/57.2958/3600)^2,(0.1/57.2958/3600)^2,0.00098^2,0.00098^2,0.00098^2,0.02^2,0.02^2,0.02^2,0.002^2,0.002^2,0.002^2,(0.1/57.2958)^2,0.05^2,(0.02/60/57.2958)^2,(0.02/60/57.2958)^2,50^2,0.25^2,0.25^2,0.25^2]);%误差方差初始值
%系统误差初始值X1-惯导，X2-多普勒，X3-磁航向仪，X4-深度计，X5-GPS
X1=zeros(18,1);X2=zeros(6,1);X3=zeros(1);X4=zeros(1);X5=zeros(6,1);
X1(10)=0.1/57.2958/3600*randn(1);X1(11)=0.1/57.2958/3600*randn(1);X1(12)=0.1/57.2958/3600*randn(1);X1(16)=(10^-4)*randn(1);X1(17)=(10^-4)*randn(1);X1(18)=(10^-4)*randn(1);
X2(4,1)=0.002;X2(5,1)=0.002;X2(6,1)=0.002;
%系统误差初始值
XI1=zeros(7201,1);XI2=zeros(7201,1);XI4=zeros(7201,1);XI5=zeros(7201,1);XI6=zeros(7201,1);XI7=zeros(7201,1);XI8=zeros(7201,1);XI9=zeros(7201,1);
XI1(1)=0;XI2(1)=0;XI3(1)=0;XI4(1)=0;XI5(1)=0;XI6(1)=0;XI7(1)=0;XI8(1)=0;XI9(1)=0;
XI10(1)=0;XI11(1)=0;XI12(1)=0;XI13(1)=0;XI14(1)=0;XI15(1)=0;XI16(1)=0;XI17(1)=0;XI18(1)=0;%滤波后惯导误差初始值
XK=zeros(32,1);%集中滤波状初值
O63=zeros(6,3);O3=zeros(3);I3=eye(3);I6=eye(6);I32=eye(32);O18=zeros(1,8);O19=zeros(1,9);O12=zeros(1,2);O115=zeros(1,15);O612=zeros(6,12);
O1814=zeros(18,14);O618=zeros(6,18);O68=zeros(6,8);O124=zeros(1,24);O17=zeros(1,7);O125=zeros(1,25);O16=zeros(1,6);O626=zeros(6,26);
O1811=zeros(18,11);O69=zeros(6,9);O112=zeros(1,12);O113=zeros(1,13);O614=zeros(6,14);O39=zeros(3,9);O93=zeros(9,3);O38=zeros(3,8);%构造零矩阵、单元矩阵5
Vn=10.6066;Ve=10.6066;Vd=0;%真实速度
%量测矩阵
H11=[0,Vd,-Ve;-Vd,0,Vn;Ve,-Vn,0];H1=[O3,I3,H11,O39];
H2=[O18,1,O19];HM=-1;
H3=[O12,1,O115];HH=-1;
H4=[I6,O612];HG=-I6;
%量测矩阵
GD=[I3;O3];%多普勒G阵元素
GM=1;GH=1;%磁航向仪G阵,深度计G阵
GG=I6;%GPS-G阵
for i=1:7200  
    f1=45/57.2958+X1(10)+X1(13);f2=X1(11)+X1(14);f3=X1(12)+X1(15);%f1-航向角，f2-横滚角，f3-俯仰角
    Cnb=[cos(f1)*cos(f3),sin(f1)*cos(f3),-sin(f3)
    cos(f1)*sin(f3)*sin(f2)-sin(f1)*cos(f2),sin(f1)*sin(f3)*sin(f2)+cos(f1)*cos(f2),cos(f3)*sin(f2)
    cos(f1)*sin(f3)*cos(f2)+sin(f1)*sin(f2),sin(f1)*sin(f3)*cos(f2)-cos(f1)*sin(f2),cos(f3)*cos(f2)];%姿态转换矩阵
    Cbn=inv(Cnb);
    HD1=[-Cbn(1,1)*15,0,0; -Cbn(2,1)*15,0,0;-Cbn(3,1)*Vn,0,0];HD=[-Cbn,HD1];%多普勒量测矩阵
    GI=[O63,O63,O63;-Cbn,O3,O3;O3,O3,O3;O3,I3,O3;O3,O3,I3];%惯导G阵元素
    G=[GI,O1811;O69,GD,O68;O112,GM,O17;O113,GH,O16;O614,GG];M1=G*Q*G';%集中滤波器G阵，
    HK=[H1,HD,O38;H2,O16,HM,O17;H3,O17,HH,O16;H4,O68,HG];%集中滤波量测阵
    wd=wd+10.6066/Re;jd=jd+10.6066/Re;
    Re=6378245*(1-sin(wd)^2/298.3);%地球半径换算
    %惯导F阵元素
    FI=zeros(18);
    FI(1,4)=1/Re;FI(2,1)=Ve*sec(wd)*tan(wd)/Re;FI(2,5)=sec(wd)/Re; FI(3,6)=1;FI(4,1)=-(2*wie*cos(wd)+Ve*(sec(wd))^2/Re)*Ve;
    FI(4,4)=0;FI(4,5)=-(2*wie*sin(wd)+Ve*tan(wd)/Re); FI(4,6)=Vn/Re;FI(4,8)=0; FI(4,9)=0;FI(4,16)=Cbn(1,1);
    FI(4,17)=Cbn(1,2);FI(4,18)=Cbn(1,3); FI(5,1)=2*wie*Vn*cos(wd)+Ve*Vn*(sec(wd))^2/Re-2*wie*Vd*sin(wd);
    FI(5,4)=2*wie*sin(wd)+Ve*tan(wd)/Re;FI(5,5)=Vn*tan(wd)/Re+Vd/Re; FI(5,7)=0;FI(5,9)=0;
    FI(5,16)=Cbn(2,1);FI(5,17)=Cbn(2,2); FI(5,18)=Cbn(2,3);FI(6,1)=2*wie*Ve*sin(wd);
    FI(6,4)=-2*Vn/Re;FI(6,5)=-2*(wie*cos(wd)+Ve/Re); FI(6,7)=0;FI(6,8)=0; FI(6,16)=Cbn(3,1);FI(6,17)=Cbn(3,2);FI(6,18)=Cbn(3,3);
    FI(7,1)=-wie*sin(wd);FI(7,5)=1/Re;FI(7,8)=-(wie*sin(wd)+Ve*tan(wd)/Re); FI(7,9)=Vn/Re;FI(7,10)=-Cbn(1,1);FI(7,11)=-Cbn(1,2);
    FI(7,12)=-Cbn(1,3);FI(7,13)=-Cbn(1,1);FI(7,14)=-Cbn(1,2);FI(7,15)=-Cbn(1,3);FI(8,4)=1/Re;FI(8,7)=wie*sin(wd)+Ve*tan(wd)/Re; 
    FI(8,9)=wie*cos(wd)+Ve/Re; FI(8,10)=-Cbn(2,1);FI(8,11)=-Cbn(2,2); FI(8,12)=-Cbn(2,3);FI(8,13)=-Cbn(2,1);
    FI(8,14)=-Cbn(2,2);FI(8,15)=-Cbn(2,3); FI(9,1)=-(wie*cos(wd)+Ve*(sec(wd))^2/Re); FI(9,5)=-tan(wd)/Re;FI(9,7)=-Vn/Re;
    FI(9,8)=-(wie*cos(wd)+Ve/Re); FI(9,10)=-Cbn(3,1);FI(9,11)=-Cbn(3,2); FI(9,12)=-Cbn(3,3);FI(9,13)=-Cbn(3,1);
    FI(9,14)=-Cbn(3,2);FI(9,15)=-Cbn(3,3);FI(13,13)=-1/tg;FI(14,14)=-1/tg;FI(15,15)=-1/tg;
    FD=diag([-1/td,-1/td,-1/td,0,0,0]);%多普勒F阵元素
    FM=-1/tm;%磁航向仪F阵
    FH=-1/th;%深度计F阵
    FG=diag([-1/tgps,-1/tgps,-1/tgps,-1/tgps,-1/tgps,-1/tgps]);%GPS-F阵
    %各系统噪声阵WI-惯导，WD-多普勒，WM-磁航向仪，WH-深度计，WG-GPS
    WI=diag([0.1/57.2958/60,0.1/57.2958/60,0.1/57.2958/60,1.4142*0.1/57.2958/3600/60,1.4142*0.1/57.2958/3600/60,1.4142*0.1/57.2958/3600/60,10^-4*g,10^-4*g,10^-4*g])*randn(9,1);
    WD=sqrt(1/150)*diag([0.02,0.02,0.02])*randn(3,1);
    WM=0.1/57.2958*sqrt(1/1500)*randn(1);
    WH=0.05*sqrt(1/500)*randn(1);
    WG=sqrt(1/50)*diag([0.01/60/57.2958,0.01/60/57.2958,20,0.25,0.25,0.25])*randn(6,1);
    %量测噪声阵
    VD=0.002*randn(3,1);VM=0.1/60/57.2958*randn(1);VH=0.005*randn(1);VG=diag([0.001/60/57.2958,0.001/60/57.2958,10,0.04,0.04,0.04])*randn(6,1);%量测噪声阵
    X1=X1+FI*X1+GI*WI;X2=X2+FD*X2+GD*WD;X3=X3+FM*X3+GM*WM;X4=X4+FH*X4+GH*WH;X5=X5+FG*X5+GG*WG;%误差求取X1-惯导，X2-多普勒，X3-磁航向仪，X4-深度计，X5-GPS
    ZD=[H1,HD]*[X1;X2]+VD;ZM=X1(9,1)-X3+VM;ZH=X1(3,1)-X4+VH;
    ZG=[X1(1,1)-X5(1,1);X1(2,1)-X5(2,1);X1(3,1)-X5(3,1);X1(4,1)-X5(4,1);X1(5,1)-X5(5,1);X1(6,1)-X5(6,1)]+VG;
    ZK=[ZD;ZM;ZH;ZG];%量测值
    F=[FI,O1814;O618,FD,O68;O124,FM,O17;O125,FH,O16;O626,FG];%集中滤波器F阵，
    %滤波递推算法
    FK=I32+F+F*F/2;M1=G*Q*(G');M2=F*M1+(F*M1)';M3=F*M2+(F*M2)';QK=M1+M2/2+M3/6;%FK,QK-等效离散一步转移阵、系统噪声方差阵，见《卡尔曼。。。》42-45
    XK1=FK*XK;%状态一步预测
    PK1=FK*PK*(FK')+QK;%一步预测方差
    N1=HK*PK1*(HK')+RK;N2=inv(N1);KK=PK1*(HK')*N2;%滤波增益
    PK=(I32-KK*HK)*PK1;%估计均方误差
    XK=XK1+KK*(ZK-HK*XK1);%状态估计
    XI1(i+1)=(X1(1)-XK(1))*Re;XI2(i+1)=(X1(2)-XK(2))*Re;XI4(i+1)=X1(4)-XK(4);XI5(i+1)=X1(5)-XK(5);XI6(i+1)=X1(6)-XK(6);
    XI7(i+1)=X1(7)-XK(7);XI8(i+1)=X1(8)-XK(8);XI9(i+1)=X1(9)-XK(9);
    XI13(i+1)=X1(13)-XK(13);XI14(i+1)=X1(14)-XK(14);XI15(i+1)=X1(15)-XK(15);
    XI16(i+1)=X1(16)-XK(16);XI17(i+1)=X1(17)-XK(17);XI18(i+1)=X1(18)-XK(18);
    %滤波之输出，XI1-纬度，XI2-经度，XI4、XI5、XI6=东，北、地向速度，XI7、XI8、XI9-失准角，
    %XI10-XI15陀螺误差，XI16-XI18加速度计误差
end
i=1:1:7200;
subplot(4,2,1);plot(i,XI1(i));
subplot(4,2,2);plot(i,XI2(i));
subplot(4,2,3);plot(i,XI4(i));
subplot(4,2,4);plot(i,XI5(i));
subplot(4,2,5);plot(i,XI6(i));
subplot(4,2,6);plot(i,XI7(i));
subplot(4,2,7);plot(i,XI8(i));
subplot(4,2,8);plot(i,XI9(i)),