close all;
clc;
clear all;
Rad_Deg = 0.01745329;
hn=0.1;
betx =2.146/2;
bety=2.146/2;
betz=2.146/2;
Tn=1000/hn;
fid=fopen('dat.txt','r');
data=load('data.txt','r');
% Cnb(1,1) = cos(roll)*cos(yaw)+sin(roll)*sin(pitch)*sin(yaw);
% Cnb(1,2) = -cos(roll)*sin(yaw)+sin(roll)*sin(pitch)*cos(yaw);
% Cnb(1,3) = -sin(roll)*cos(pitch);
% Cnb(2,1) = cos(pitch)*sin(yaw);
% Cnb(2,2) = cos(pitch)*cos(yaw);
% Cnb(2,3) = sin(pitch);
% Cnb(3,1) = sin(roll)*cos(yaw)-cos(roll)*sin(pitch)*sin(yaw);
% Cnb(3,2) = -sin(roll)*sin(yaw)-cos(roll)*sin(pitch)*cos(yaw);
% Cnb(3,3) = cos(roll)*cos(pitch);
% Tbn=Cnb';
bx=data(:,1);
by=data(:,2);
bz=data(:,3);
X=zeros(9,1);
X(1)=2*Rad_Deg;
X(2)=3*Rad_Deg;
X(3)=-4*Rad_Deg;
X(4)=0.4*Rad_Deg;
X(5)=0.4*Rad_Deg;
X(6)=0.4*Rad_Deg;
X(7)=0;
X(8)=0;
X(9)=0;
Q77=4*betx*betx*betx*1e-2*hn;
Q88=4*bety*bety*bety*1e-2*hn;
Q99=4*betz*betz*betz*1e-2*hn;
Q=[0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 Q77 0 0;
0 0 0 0 0 0 0 Q88 0;
0 0 0 0 0 0 0 0 Q99];
R=[2*1e-3 0 0;
0 2*1e-3 0;
0 0 2*1e-3];
P=[0.5 0 0 0 0 0 0 0 0;
0 0.5 0 0 0 0 0 0 0;
0 0 0.5 0 0 0 0 0 0;
0 0 0 0.5 0 0 0 0 0;
0 0 0 0 0.5 0 0 0 0;
0 0 0 0 0 0.5 0 0 0;
0 0 0 0 0 0 0.5 0 0;
0 0 0 0 0 0 0 0.5 0;
0 0 0 0 0 0 0 0 0.5];
for k=1:Tn;
raw=fscanf(fid,'%f',[18,1]);
H=[0 raw(9) -raw(8) 0 raw(9) -raw(8) 0 0 0;
-raw(9) 0 raw(7) -raw(9) 0 raw(7) 0 0 0;
raw(8) -raw(7) 0 raw(8) -raw(7) 0 0 0 0];
A=[0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 1 0;
0 0 0 0 0 0 0 0 1;
0 0 0 -betx*betx 0 0 -2*betx 0 0;
0 0 0 0 -bety*bety 0 0 -2*bety 0;
0 0 0 0 0 -betz*betz 0 0 -2*betz];
B=zeros(9,9);
B(7,7)=1;
B(8,8)=1;
B(9,9)=1;
[F,G]=c2d(A,B,hn);
F;
G;
FT = F';
HT = H';
BB=P*HT;
Z=[raw(7)-raw(1);
raw(8)-raw(2);
raw(9)-raw(3)];
z(k)=Z(1);
XK = F*X;
PK = F*P*FT+G*Q*G';
K = PK*HT*inv(H*PK*HT+R);
X = XK+K*(Z-H*XK);
P = (eye(9)-K*H)*PK;
pitchT=raw(13);
rollT=raw(14);
yawT=raw(15);
pitchB=raw(16);
rollB=raw(17);
yawB=raw(18);
Cnm(1,1) = cos(rollT)*cos(yawT)+sin(rollT)*sin(pitchT)*sin(yawT);
Cnm(1,2) = -cos(rollT)*sin(yawT)+sin(rollT)*sin(pitchT)*cos(yawT);
Cnm(1,3) = -sin(rollT)*cos(pitchT);
Cnm(2,1) = cos(pitchT)*sin(yawT);
Cnm(2,2) = cos(pitchT)*cos(yawT);
Cnm(2,3) = sin(pitchT);
Cnm(3,1) = sin(rollT)*cos(yawT)-cos(rollT)*sin(pitchT)*sin(yawT);
Cnm(3,2) = -sin(rollT)*sin(yawT)-cos(rollT)*sin(pitchT)*cos(yawT);
Cnm(3,3) = cos(rollT)*cos(pitchT);
Cms(1,1) = cos(X(2)+X(5))*cos(-X(3)-X(6))+sin(X(2)+X(5))*sin(X(1)+X(4))*sin(-X(3)-X(6));
Cms(1,2) = -cos(X(2)+X(5))*sin(-X(3)-X(6))+sin(X(2)+X(5))*sin(X(1)+X(4))*cos(-X(3)-X(6));
Cms(1,3) = -sin(X(2)+X(5))*cos(X(1)+X(4));
Cms(2,1) = cos(X(1)+X(4))*sin(-X(3)-X(6));
Cms(2,2) = cos(X(1)+X(4))*cos(-X(3)-X(6));
Cms(2,3) = sin(X(1)+X(4));
Cms(3,1) = sin(X(2)+X(5))*cos(-X(3)-X(6))-cos(X(2)+X(5))*sin(X(1)+X(4))*sin(-X(3)-X(6));
Cms(3,2) = -sin(X(2)+X(5))*sin(-X(3)-X(6))-cos(X(2)+X(5))*sin(X(1)+X(4))*cos(-X(3)-X(6));
Cms(3,3) = cos(X(2)+X(5))*cos(X(1)+X(4));
Cns = Cms*Cnm;
CnbT(1,1) = cos(rollB)*cos(yawB)+sin(rollB)*sin(pitchB)*sin(yawB);
CnbT(1,2) = -cos(rollB)*sin(yawB)+sin(rollB)*sin(pitchB)*cos(yawB);
CnbT(1,3) = -sin(rollB)*cos(pitchB);
CnbT(2,1) = cos(pitchB)*sin(yawB);
CnbT(2,2) = cos(pitchB)*cos(yawB);
CnbT(2,3) = sin(pitchB);
CnbT(3,1) = sin(rollB)*cos(yawB)-cos(rollB)*sin(pitchB)*sin(yawB);
CnbT(3,2) = -sin(rollB)*sin(yawB)-cos(rollB)*sin(pitchB)*cos(yawB);
CnbT(3,3) = cos(rollB)*cos(pitchB);
errC=CnbT*inv(Cns);
data(k)=X(1)/Rad_Deg;
datb(k)=X(2)/Rad_Deg;
datc(k)=X(3)/Rad_Deg;
datd(k)=X(4)/Rad_Deg;
date(k)=X(5)/Rad_Deg;
datf(k)=X(6)/Rad_Deg;
% datg(k)=-errC(3,2)/Rad_Deg;
% dath(k)= errC(3,1)/Rad_Deg;
% dati(k)=-errC(2,1)/Rad_Deg;
datg(k)= errC(3,2)/Rad_Deg; %%by me
dath(k)= errC(3,1)/Rad_Deg;
dati(k)= errC(2,1)/Rad_Deg;
end;
fclose(fid);
t=1:Tn;
figure(1);
plot(t,data(t));
figure(2)
plot(t,datb(t));
figure(3)
plot(t,datc(t));
% figure(2);
% subplot(1,3,1);
% plot(t,datd);
% subplot(1,3,2);
% plot(t,date);
% subplot(1,3,3);
% plot(t,datf);
%
figure(4);
plot(t,datg(t),'b',t,-bx(t)/Rad_Deg,'r');
figure(5)
plot(t,dath(t),'b',t,by(t)/Rad_Deg,'r');
figure(6)
plot(t,dati(t),'b',t,bz(t)/Rad_Deg,'r');
评论5