%主題：基于chen的立方體六加速度計配置角速度誤差和線加速度誤差曲線仿真
%條件：沒有考慮加速度計的常值零偏和隨機噪聲誤差干擾
%日期：2007.12.5
%結(jié)果：成功
%參考文獻：劉金鋒---無陀螺微慣性測量單元的配置方式研究，2007，3

clear all
close all
clc

l=0.04;
T=0.01;
total=100;
r=[0,0,-l;0,-l,0;-l,0,0;l,0,0;0,l,0;0,0,l];
sita=1/sqrt(2)*[1,1,0;1,0,1;0,1,1;0,-1,1;-1,0,1;-1,1,0];
A=zeros(6,1);
J1=1/sqrt(2)*[1,-1,0,0,1,-1;-1,0,1,-1,0,-1;0,1,-1,-1,1,0];
J2=1/sqrt(2)*[1,1,0,0,-1,-1;1,0,1,-1,0,1;0,1,1,1,1,0];
w=[0,2,3]';
f=[0,0,0]';
for i=1:total/T;
    for j=1:6;
        ssita=[sita(j,1),sita(j,2),sita(j,3)]';
        R=[0,-r(j,3),r(j,2);r(j,3),0,-r(j,1);-r(j,2),r(j,1),0];
        rr=[r(j,1),r(j,2),r(j,3)]';
        wwx=2*sin(0.4*i*T);wwy=2*cos(0.6*i*T);wwz=3+0.0001*(i*T)^2;
        WW=[0,-wwz,wwy;wwz,0,-wwx;-wwy,wwx,0];
        dwx=2*0.4*cos(0.4*i*T);dwy=-2*0.6*sin(0.6*i*T);dwz=2*0.0001*i*T;
        ffx=3*sin(i*T)*cos(i*T);ffy=cos(2*i*T)*cos(i*T);ffz=2*cos(2*i*T)*cos(0.5*i*T);
        Q1=[dwx,dwy,dwz]';Q2=[ffx,ffy,ffz]';
        A(j)=ssita'*R'*Q1+ssita'*Q2+ssita'*WW*WW*rr;
    end;
    w=w+T/(2*l)*J1*A;
    wx(i)=w(1);wy(i)=w(2);wz(i)=w(3);
    delta_wx(i)=wx(i)-wwx;delta_wy(i)=wy(i)-wwy;delta_wz(i)=wz(i)-wwz;
    P=[w(2)*w(3),w(1)*w(3),w(1)*w(2)]';
    f=0.5*J2*A+l*P;
    fx(i)=f(1);fy(i)=f(2);fz(i)=f(3);
    delta_fx(i)=fx(i)-ffx;delta_fy(i)=fy(i)-ffy;delta_fz(i)=fz(i)-ffz;
end
i=1:total/T;
figure(1);
subplot(3,1,1)
plot(i*T,delta_wx);
ylabel('△ωx(deg/s)');
 xlabel('t/s');
grid on;

subplot(3,1,2)
plot(i*T,delta_wy);
 ylabel('△ωy(deg/s)');
 xlabel('t/s');
grid on;

subplot(3,1,3)
plot(i*T,delta_wz);
 ylabel('△ωz(deg/s)');
 xlabel('t/s');
grid on;

figure(2);
subplot(3,1,1)
plot(i*T,delta_fx);
ylabel('△fx(m/s2)');
 xlabel('t/s');
grid on;

subplot(3,1,2)
plot(i*T,delta_fy);
 ylabel('△fy(m/s2)');
 xlabel('t/s');
grid on;

subplot(3,1,3)
plot(i*T,delta_fz);
 ylabel('△fz(m/s2)');
 xlabel('t/s');
grid on;