ekf2.m

通过MATLAB实现卡尔曼滤波并应用于初始对准

m_Fxb=0.5;m_Fyb=-0.52;m_Fzb=0.32;                         
m_Wibx=0;m_Wiby=0.000062884;m_Wibz=0.000036879;
syms tao;
tao=0.01;
g=9.80;
Fi0=30.39*pi/180;
We=0.000072921;
Wn=We*cos(Fi0);
Wu=We*sin(Fi0);
T0=zeros(3,3);
Vxp0=0.1;Vyp0=0.1;Vzp0=0;    %速度初始值不为零，根据卡尔曼滤波理论
delta_E=0;delta_N=0;delta_U=0;
T0(1,1)=sec(Fi0)/g/We*(m_Wibz*m_Fyb-m_Wiby*m_Fzb);
T0(1,2)=sec(Fi0)/g/We*(m_Wibz*m_Fzb-m_Wibz*m_Fxb);
T0(1,3)=sec(Fi0)/g/We*(m_Wiby*m_Fxb-m_Wibx*m_Fyb);
T0(2,1)=m_Fxb/g*tan(Fi0)+m_Wibx/We*sec(Fi0);
T0(2,2)=m_Fyb/g*tan(Fi0)+m_Wiby/We*sec(Fi0);
T0(2,3)=m_Fzb/g*tan(Fi0)+m_Wibz/We*sec(Fi0);
T0(3,1)=-m_Fxb/g;
T0(3,2)=-m_Fyb/g;
T0(3,3)=-m_Fzb/g;
Y=[pi/180 ;pi/180 ;pi/180];
T=0;
temp_E=0;
temp_N=0;
X0=[0.1 0.1 pi/180 pi/180 pi/180 1e-4*g 1e-4*g 0.02*pi/180/3600 0.02*pi/180/3600 0.02*pi/180/3600]';
%X0=zeros(10,1);
%Q=diag([0,0,0,0,0 (50e-6*g)^2,(50e-6*g)^2,(0.01*pi/180/3600)^2,(0.01*pi/180/3600)^2,(0.01*pi/180/3600)^2]);%做了一下修改顺序颠倒了
Q=diag([(50e-6*g)^2,(50e-6*g)^2,(0.01*pi/180/3600)^2,(0.01*pi/180/3600)^2,(0.01*pi/180/3600)^2,0,0,0,0,0 ]);
P0=diag([0.1 0.1 pi/180 pi/180 pi/180 (100e-6*g)^2 (100e-6*g)^2 (0.02*pi/180/3600)^2 (0.02*pi/180/3600)^2 (0.02*pi/180/3600)^2]);
R=diag([0.1 0.1]);
H=[eye(2,2) zeros(2,8)];
for i=1:1
m_Fp=T0*[m_Fxb-delta_E;m_Fyb-delta_N;m_Fzb-delta_U];%修正加速度
m_Wibx=m_Wibx-temp_E;%修正陀螺
m_Wiby=m_Wiby-temp_N;%修正陀螺

Wiep=T0*[m_Wibx;m_Wiby;m_Wibz];
Wiepx=Wiep(1);Wiepy=Wiep(2);Wiepz=Wiep(3);
Vxp=Vxp0+(m_Fp(1)+2*Wiepz*Vyp0-2*Wiepy*Vzp0)*tao;
Vyp=Vyp0+(m_Fp(2)-2*Wiepz*Vxp0+2*Wiepx*Vzp0)*tao;
Vzp=Vzp0+(m_Fp(3)-g+2*Wiepy*Vxp0-2*Wiepx*Vyp0)*tao;
C=[T0(2:3,2:3) zeros(2,3);zeros(3,2) T0];
F=[0 2*Wu 0 -g 0;-2*Wu 0 g 0 0;0 0 0 Wu -Wn;0 0 -Wu 0 0;0 0 Wn 0 0];
AA=[F C;zeros(5,5) zeros(5,5)];
CC=[eye(2,2) zeros(2,8)];
BB=eye(10,10);
DD=zeros(2,10);
sys=ss(AA,BB,CC,DD);
[Ad,Bd,Cd,Dd]=c2dm(AA,BB,CC,DD,tao);
Vxp=Vxp-Vxp0;
Vyp=Vyp-Vyp0;
Vzp=Vzp0-Vzp0;

Zk=[Vxp;Vyp];
Xkk_1=Ad*X0;
Pkk_1=Ad*P0*Ad'+Q;
Kk=Pkk_1*H'*inv(H*Pkk_1*H'+R);
Xk=Xkk_1+Kk*(Zk-H*Xkk_1);
Pk=(eye(10,10)-Kk*H)*Pkk_1;

	m_FuYang=asin(T0(3,2));							
    m_QingXie=atan(-T0(3,1)/T0(3,3));						
		if(T0(3,3)<0&&m_QingXie<0)
		m_QingXie=m_QingXie+pi;
    elseif(T0(3,3)<0&&m_QingXie>0)
		m_QingXie=m_QingXie-pi;	
    end
	m_GeWang=atan(-T0(1,2)/T0(2,2));
	if(T0(2,2)>0&&m_GeWang<0)
		m_GeWang=m_GeWang+2*pi;
    end
	if(T0(2,2)<0)
		m_GeWang=m_GeWang+pi;						
    end
%delta_GeWang=tan(m_FuYang)*(Xk(4)*sin(m_GeWang)+Xk(3)*cos(m_GeWang))+Xk(5);
%delta_FuYang=-Xk(3)*sin(m_GeWang)+Xk(4)*cos(m_GeWang);
%delta_QingXie=(Xk(4)*sin(m_GeWang)+Xk(3)*cos(m_GeWang))/cos(m_FuYang);    
%m_FuYang=m_FuYang-delta_FuYang;
%m_QingXie=m_QingXie-delta_QingXie;
%m_GeWang=m_GeWang-delta_GeWang;     %姿态修正
m_FuYang=m_FuYang-Xk(3);
m_QingXie=m_QingXie-Xk(4);
m_GeWang=m_GeWang-Xk(5);

T0(1,1)=cos(m_QingXie)*cos(m_GeWang)-sin(m_QingXie)*sin(m_FuYang)*sin(m_GeWang);
T0(1,2)=-cos(m_FuYang)*sin(m_GeWang);
T0(1,3)=sin(m_QingXie)*cos(m_GeWang)+cos(m_QingXie)*sin(m_FuYang)*sin(m_GeWang);
T0(2,1)=cos(m_QingXie)*sin(m_GeWang)+sin(m_QingXie)*sin(m_FuYang)*sin(m_GeWang);
T0(2,2)=cos(m_FuYang)*cos(m_GeWang);
T0(2,3)=sin(m_QingXie)*sin(m_GeWang)-cos(m_QingXie)*sin(m_FuYang)*cos(m_GeWang);
T0(3,1)=-sin(m_QingXie)*cos(m_FuYang);
T0(3,2)=sin(m_FuYang);
T0(3,3)=cos(m_QingXie)*cos(m_FuYang);
Vxp0=Vxp;Vyp0=Vyp;Vzp0=Vzp;P0=Pk;X0=Xk;
delta_E=Xk(8);
delta_N=Xk(9);
delta_U=Xk(10);

temp=[Xk(3);Xk(4);Xk(5)];
temp_E=Xk(6);
temp_N=Xk(7);
Y=[Y temp];
T=[T tao*i];
end