Алгоритмы БИНС, Алгоритмы Бесплатформенной Инерциальной Навигационной Системы Опции

[wookX]

Имеется алгоритм Бесплатформенной Инерциальной Навигационной Системы. Состоит из двух файлов.
Если кто в этом разбирается, не могли бы подсказать, алгоритм функционирует или всеже нет?

Файл первый:
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

И файл второй:
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

[wookX]

INS Toolbox достать не имею возможноости

А первый файл со вторым связан посредством строки:

Код

[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);

То есть в первом файле начальные условия и выходные данные, а во втором именно уравнения которые необходимо проинтегрировать.

Интегрирование происходит посредством функции

Код

ode45

попозже выложу модифицированный код, построенный полностью на перемножении матриц.
Но результаты почему то уж очень отличаются.