kalman.m

捷联惯导初始对准Kalman滤波程序

function y=kalman()
%使用Kalman滤波方法设计的SINS初始对准仿真程序，采用的方法详见 捷联惯导系统初始对准滤波技术研究.pdf P34
%
%试验中曲线一直在0附近（一开始就马上趋于0了以后几乎不动），其原因是输出序列前项与后项的比值很大，相差1000倍以上，在曲线上无法
%反映这么大的差距造成的
%又发现在使用滤波的时候原来连续的模型没有进行离散化就用了，不知道是不是这个问题引起的上面的问题，原因就是这个，但是计算
%真实的状态时不知道是使用连续模型还是离散模型，此外发现xout(:,5)的输出波形有时会发生相位相反的情况
N=1000;%参数设置和模型建立
omega=7.2916*10^-5;%地球自转角速度
g=9.84;%重力加速度
ac=100*10^-6*g;ar=50*10^-6*g;%加速度计初始偏差和随机偏差
deg=1/180*pi;%一度对应的的弧度
pc=0.02*pi/180/3600;pr=0.01*pi/180/3600;%陀螺仪常值漂移和随机漂移                      %化不化成标准单位呢，一直很困惑的，最后选择了化,还是应该化的
%deg=1;
%pc=0.02;pr=0.01;
phi=pi/4;%初始纬度
xr0=[0.1 0.1 deg deg deg zeros(1,5)]';%状态初值，真实值
xe0=zeros(1,10)';%状态估计初值
Q=diag([ar^2 ar^2 pr^2 pr^2 pr^2 zeros(1,5)]);%系统噪声协方差矩阵
P0=diag([0.1^2 0.1^2 deg^2 deg^2 deg^2 zeros(1,5)]);%
R=diag([0.1^2 0.1^2]);%观测噪声协方差矩阵
Ou=omega*sin(phi);On=omega*cos(phi);
F=[0 2*Ou 0 -g 0;
    -2*Ou 0 g 0 0;
    0 0 0 Ou -On;
    0 0 -Ou 0 0;
    0 0 On 0 0];
C=eye(5);%初始这个矩阵可以取为单位阵，即认为两个坐标系重合，之后需要使用估计出来的失准角进行修正
A=[F C;
    zeros(5,5) zeros(5,5)];
B=eye(10);
H=[eye(2) zeros(2,8)];D=zeros(2,10);%这里虽然设了B和D实际上都没有用，只是为了能够使用ss函数才设成D=zeros(2,10)
G=ss(A,B,H,D);
T=1;
Gd=c2d(G,T);
PHI=Gd.a;%可以使用PHI=expm(A*T)
xout=zeros(10,N);%估计输出

%开始计算了
xrk_1=xr0;%状态的真实值
xk_1=xe0;
Pk_1=P0;
for i=1:N
    Z=H*xrk_1+(mvnrnd([0;0],R,1))';%当前时刻观测值
    Pkk=PHI*Pk_1*PHI'+Q;
    K=Pkk*H'*inv(H*Pkk*H'+R);
    Pk=(eye(10)-K*H)*Pkk;
    xk=PHI*xk_1;%一步预测
    xk=xk+K*(Z-H*xk);%状态估计
    xk=xk;
    xout(:,i)=xk;
    xk_1=xk;
    Pk_1=Pk;
    %根据失准角修正C
%     dH=xk(5);dksi=xk(3);dtheta=xk(4);%三个欧拉角与三个失准角相对应
%     C11=cos(dH)*cos(dtheta)+sin(dH)*sin(dksi)*sin(dtheta);C12=sin(dH)*cos(dksi);C13=cos(dH)*sin(dtheta)-sin(dH)*sin(dksi)*cos(dtheta);
%     C21=cos(dH)*sin(dksi)*sin(dtheta)-sin(dH)*cos(dtheta);C22=cos(dH)*cos(dksi);C23=-sin(dH)*sin(dtheta)-cos(dH)*sin(dksi)*cos(dtheta);
%     C31=-cos(dksi)*sin(dtheta);C32=sin(dksi);C33=cos(dksi)*cos(dtheta);
%     C=[C11 C12 0 0 0;
%        C21 C22 0 0 0
%        0 0 C11 C12 C13
%        0 0 C21 C22 C23
%        0 0 C31 C32 C33];
%     A=[F C;
%        zeros(5,5) zeros(5,5)];
   %PHI=expm(A*T);
   xrk=A*xrk_1+(mvnrnd(zeros(10,1),Q,1))';%这里计算实际的状态究竟是用离散化之后的模型还是用连续模型呢，用连续模型得到的效果还可以，离散的不行？
   xrk_1=xrk;
end
t=1:N;
% plot(t,xout(1,:),'-*',t,xout(2,:),'-v');
% figure;
% plot(t,xout(3,:)*3600*180/pi,'-*',t,xout(4,:)*3600*180/pi,'-v');%化成秒（角）输出
% figure;
% plot(t,xout(5,:)*3600*180/pi,'-o');
plot(t,xout(1,:),t,xout(2,:));
figure;
plot(t,xout(3,:)*3600*180/pi,t,xout(4,:)*3600*180/pi);%化成秒（角）输出
figure;
plot(t,xout(5,:)*3600*180/pi);