Имеется алгоритм Бесплатформенной Инерциальной Навигационной Системы. Состоит из двух файлов.
Если кто в этом разбирается, не могли бы подсказать, алгоритм функционирует или всеже нет?
Файл первый:
ins.m
Код
%%%Load file with accelerations and angular rates
load 'initial.mat';
g=9.81;
tmin=1;
tmax=20000;
t1=1:2:tmax;
% %%creating individual arrays
'Inside the loop'
k=0;
for i=1:tmax
k=k+1;
ax(k)=(AccX.signals.values(k))*g;
ay(k)=(AccY.signals.values(k))*g;
az(k)=(AccZ.signals.values(k))*g;
p(k)=P.signals.values(k);
q(k)=Q.signals.values(k);
r(k)=R.signals.values(k);
end
%% Initialising values
theta(1)=0.1095;
phi(1)=0:
psi(1)=0;
e00=cos(psi0/2.0)*cos(theta0/2.0)*cos(phi0/2.0)+sin(psi0/2.0)*sin(theta0/2.0)*sin(phi0/2.0);
e10=cos(psi0/2.0)*cos(theta0/2.0)*sin(phi0/2.0)-sin(psi0/2.0)*sin(theta0/2.0)*cos(phi0/2.0);
e20=cos(psi0/2.0)*sin(theta0/2.0)*cos(phi0/2.0)+sin(psi0/2.0)*cos(theta0/2.0)*sin(phi0/2.0);
e30=-cos(psi0/2.0)*sin(theta0/2.0)*sin(phi0/2.0)+sin(psi0/2.0)*cos(theta0/2.0)*cos(phi0/2.0);
Vt=45;
Alpha=0.1292;
Beta=-0.0663;
U0 = Vt/sqrt(1+(tan(Alpha))^2+(tan(Beta))^2);
V0 = Vt*(tan(Beta))/sqrt(1+(tan(Alpha))^2+(tan(Beta))^2);
W0 = Vt*(tan(Alpha))/sqrt(1+(tan(Alpha))^2+(tan(Beta))^2);
%Initial conditions 1:e0, 2:e1, 3:e2, 4: e3 5:U, 6:V, 7:W, 8:X, 9:Y, 10:Z 11:Lat 12:Lon 13:H
y0(1)=e00;
y0(2)=e10;
y0(3)=e20;
y0(4)=e30;
y0(5)=U0; %Uinit
y0(6)=V0; %Vinit
y0(7)=W0; %Winit
y0(8)=0;
y0(9)=0;
y0(10)=0;
y0(11)=0.0;%Latitude
y0(12)=0.0;%Longitude
y0(13)=609.6000;%Altitude
'Integration starting'
for i=1:tmax
[T,Yy] = ode45('integrate',[t1(i) t1(i+1)],y0,[],ax(i),ay(i),az(i),p(i),q(i),r(i),theta(i),phi(i),psi(i),i);
[s1,s2]=size(Yy);
y0=Yy(s1,:);
% Equation to be solved
esq1=y0(1)*y0(1)-y0(2)*y0(2)-y0(3)*y0(3)+y0(4)*y0(4);
esqrt=sqrt(1.0-(4.0*(y0(2)*y0(4)-y0(1)*y0(3))^2));
esq2=y0(1)*y0(1)+y0(2)*y0(2)-y0(3)*y0(3)-y0(4)*y0(4);
theta(i)=(asin(-2.0*(y0(2)*y0(4)-y0(1)*y0(3))));
phi(i)=(acos(esq1/esqrt)*sign(2*(y0(3)*y0(4)+y0(1)*y0(2))));
psi(i)=(acos(esq2/esqrt)*sign(2*(y0(2)*y0(3)+y0(1)*y0(4))));
U(i)=y0(5);
V(i)=y0(6);
W(i)=y0(7);
XXX(i)=y0(8);
YYY(i)=y0(9);
ZZZ(i)=y0(10);
La(i)=y0(11);
Lo(i)=y0(12);
H(i)=y0(13);
end
load 'initial.mat';
g=9.81;
tmin=1;
tmax=20000;
t1=1:2:tmax;
% %%creating individual arrays
'Inside the loop'
k=0;
for i=1:tmax
k=k+1;
ax(k)=(AccX.signals.values(k))*g;
ay(k)=(AccY.signals.values(k))*g;
az(k)=(AccZ.signals.values(k))*g;
p(k)=P.signals.values(k);
q(k)=Q.signals.values(k);
r(k)=R.signals.values(k);
end
%% Initialising values
theta(1)=0.1095;
phi(1)=0:
psi(1)=0;
e00=cos(psi0/2.0)*cos(theta0/2.0)*cos(phi0/2.0)+sin(psi0/2.0)*sin(theta0/2.0)*sin(phi0/2.0);
e10=cos(psi0/2.0)*cos(theta0/2.0)*sin(phi0/2.0)-sin(psi0/2.0)*sin(theta0/2.0)*cos(phi0/2.0);
e20=cos(psi0/2.0)*sin(theta0/2.0)*cos(phi0/2.0)+sin(psi0/2.0)*cos(theta0/2.0)*sin(phi0/2.0);
e30=-cos(psi0/2.0)*sin(theta0/2.0)*sin(phi0/2.0)+sin(psi0/2.0)*cos(theta0/2.0)*cos(phi0/2.0);
Vt=45;
Alpha=0.1292;
Beta=-0.0663;
U0 = Vt/sqrt(1+(tan(Alpha))^2+(tan(Beta))^2);
V0 = Vt*(tan(Beta))/sqrt(1+(tan(Alpha))^2+(tan(Beta))^2);
W0 = Vt*(tan(Alpha))/sqrt(1+(tan(Alpha))^2+(tan(Beta))^2);
%Initial conditions 1:e0, 2:e1, 3:e2, 4: e3 5:U, 6:V, 7:W, 8:X, 9:Y, 10:Z 11:Lat 12:Lon 13:H
y0(1)=e00;
y0(2)=e10;
y0(3)=e20;
y0(4)=e30;
y0(5)=U0; %Uinit
y0(6)=V0; %Vinit
y0(7)=W0; %Winit
y0(8)=0;
y0(9)=0;
y0(10)=0;
y0(11)=0.0;%Latitude
y0(12)=0.0;%Longitude
y0(13)=609.6000;%Altitude
'Integration starting'
for i=1:tmax
[T,Yy] = ode45('integrate',[t1(i) t1(i+1)],y0,[],ax(i),ay(i),az(i),p(i),q(i),r(i),theta(i),phi(i),psi(i),i);
[s1,s2]=size(Yy);
y0=Yy(s1,:);
% Equation to be solved
esq1=y0(1)*y0(1)-y0(2)*y0(2)-y0(3)*y0(3)+y0(4)*y0(4);
esqrt=sqrt(1.0-(4.0*(y0(2)*y0(4)-y0(1)*y0(3))^2));
esq2=y0(1)*y0(1)+y0(2)*y0(2)-y0(3)*y0(3)-y0(4)*y0(4);
theta(i)=(asin(-2.0*(y0(2)*y0(4)-y0(1)*y0(3))));
phi(i)=(acos(esq1/esqrt)*sign(2*(y0(3)*y0(4)+y0(1)*y0(2))));
psi(i)=(acos(esq2/esqrt)*sign(2*(y0(2)*y0(3)+y0(1)*y0(4))));
U(i)=y0(5);
V(i)=y0(6);
W(i)=y0(7);
XXX(i)=y0(8);
YYY(i)=y0(9);
ZZZ(i)=y0(10);
La(i)=y0(11);
Lo(i)=y0(12);
H(i)=y0(13);
end
И файл второй:
integrate.m
Код
function INS=eqn_motion(rtime,y,flag,ax1,ay1,az1,p1,q1,r1,theta1,phi1,psi1,counter)
% linear interpolation for acceleration
% Ellipsoid WGS-84 parameters
g=9.81;
a=6378137;
e=1/298.257223563;
e2=2*e-e^2;
RM=a*(1-e2)/sqrt((1-e2*sin(y(11))^2)^3);
R1=a/sqrt(1-e2*sin(y(11))^2);
Omega=deg2rad(15/3600);
INS=zeros(13,1);
% % %Direction Cosine Matrix
dcm(1,1) = cos(theta1)*cos(psi1);
dcm(2,1) = sin(phi1)*sin(theta1)*cos(psi1)-cos(phi1)*sin(psi1);
dcm(3,1) = cos(phi1)*sin(theta1)*cos(psi1)+sin(phi1)*sin(psi1);
dcm(1,2) = cos(theta1)*sin(psi1);
dcm(2,2) = sin(phi1)*sin(theta1)*sin(psi1)+cos(phi1)*cos(psi1);
dcm(3,2) = cos(phi1)*sin(theta1)*sin(psi1)-sin(phi1)*cos(psi1);
dcm(1,3) =-sin(theta1);
dcm(2,3) = sin(phi1)*cos(theta1);
dcm(3,3) = cos(phi1)*cos(theta1);
%%%Velocity along N, E and downward velocity
[Vn;Ve;Vd]=dcm'*[y(5);y(6);y(7)];
%%% the angular rate vector between navigation frame and body frame
ww=[Omega*cos(y(11))+Ve/R1;0-Vn/RM;-Omega*sin(y(11))-Ve*tan(y(11))/R1];
%%% correction to the measured angular rates
[p1;q1;r1]=[p1;q1;r1] - dcm*ww;
% Quaternions Normalization
[y(1);y(2);y(3);y(4)]=[y(1);y(2);y(3);y(4)]/sqrt(y(1)^2+y(2)^2+y(3)^2+y(4)^2);
% Quaternions Derivatives Calculation
edot=0.5*[0 -p1 -q1 -r1;p1 0 r1 -q1;q1 -r1 0 p1;r1 q1 -p1 0]*[y(1);y(2);y(3);y(4)];
%U,V,W (acceleration centrifugal and gravitational correction)
[Udot;Vdot;Wdot]=[ax1;ay1;az1]-[p1;q1;r1]*[y(5);y(6);y(7)]+g*[-sin(theta1);cos(theta1)*sin(phi1);cos(theta1)*sin(phi1)];
%Output parameters
INS(1)=edot(1);%e0dot
INS(2)=edot(2);%e1dot
INS(3)=edot(3);%e2dot
INS(4)=edot(4);%e3dot
INS(5)=Udot;
INS(6)=Vdot;
INS(7)=Wdot;
INS(8)=Vn;%Xdot
INS(9)=Ve;%Ydot
INS(10)=Vd;%Zdot
INS(11)=Vn/RM;%Latitude
INS(12)=Ve/(R1*cos(y(11)));%Longitude
INS(13)=-Vd; %Altitude
% linear interpolation for acceleration
% Ellipsoid WGS-84 parameters
g=9.81;
a=6378137;
e=1/298.257223563;
e2=2*e-e^2;
RM=a*(1-e2)/sqrt((1-e2*sin(y(11))^2)^3);
R1=a/sqrt(1-e2*sin(y(11))^2);
Omega=deg2rad(15/3600);
INS=zeros(13,1);
% % %Direction Cosine Matrix
dcm(1,1) = cos(theta1)*cos(psi1);
dcm(2,1) = sin(phi1)*sin(theta1)*cos(psi1)-cos(phi1)*sin(psi1);
dcm(3,1) = cos(phi1)*sin(theta1)*cos(psi1)+sin(phi1)*sin(psi1);
dcm(1,2) = cos(theta1)*sin(psi1);
dcm(2,2) = sin(phi1)*sin(theta1)*sin(psi1)+cos(phi1)*cos(psi1);
dcm(3,2) = cos(phi1)*sin(theta1)*sin(psi1)-sin(phi1)*cos(psi1);
dcm(1,3) =-sin(theta1);
dcm(2,3) = sin(phi1)*cos(theta1);
dcm(3,3) = cos(phi1)*cos(theta1);
%%%Velocity along N, E and downward velocity
[Vn;Ve;Vd]=dcm'*[y(5);y(6);y(7)];
%%% the angular rate vector between navigation frame and body frame
ww=[Omega*cos(y(11))+Ve/R1;0-Vn/RM;-Omega*sin(y(11))-Ve*tan(y(11))/R1];
%%% correction to the measured angular rates
[p1;q1;r1]=[p1;q1;r1] - dcm*ww;
% Quaternions Normalization
[y(1);y(2);y(3);y(4)]=[y(1);y(2);y(3);y(4)]/sqrt(y(1)^2+y(2)^2+y(3)^2+y(4)^2);
% Quaternions Derivatives Calculation
edot=0.5*[0 -p1 -q1 -r1;p1 0 r1 -q1;q1 -r1 0 p1;r1 q1 -p1 0]*[y(1);y(2);y(3);y(4)];
%U,V,W (acceleration centrifugal and gravitational correction)
[Udot;Vdot;Wdot]=[ax1;ay1;az1]-[p1;q1;r1]*[y(5);y(6);y(7)]+g*[-sin(theta1);cos(theta1)*sin(phi1);cos(theta1)*sin(phi1)];
%Output parameters
INS(1)=edot(1);%e0dot
INS(2)=edot(2);%e1dot
INS(3)=edot(3);%e2dot
INS(4)=edot(4);%e3dot
INS(5)=Udot;
INS(6)=Vdot;
INS(7)=Wdot;
INS(8)=Vn;%Xdot
INS(9)=Ve;%Ydot
INS(10)=Vd;%Zdot
INS(11)=Vn/RM;%Latitude
INS(12)=Ve/(R1*cos(y(11)));%Longitude
INS(13)=-Vd; %Altitude
