wookX 0 2 мая, 2007 Опубликовано 2 мая, 2007 · Жалоба Имеется алгоритм Бесплатформенной Инерциальной Навигационной Системы. Состоит из двух файлов. Если кто в этом разбирается, не могли бы подсказать, алгоритм функционирует или всеже нет? Файл первый: 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 Цитата Поделиться сообщением Ссылка на сообщение Поделиться на другие сайты Поделиться
CodeWarrior1241 0 2 мая, 2007 Опубликовано 2 мая, 2007 · Жалоба Вроде нужное есть, но связи между файлами нет, они вроде не созданы работать вместе, или я что-то не рублю... Я посмотрел бегло, и не попробовал в matlab, но вроде earth rotation rate и geodic shape принимаются в счет, и модель притяжения есть. Насколько я это дело понимаю, надо интегрировать aircraft acceleration and angular rate чтобы получить позицию, скорость, и altitude. Это делается от sample values в первом файле. Вроде второй файл "создаёт" эти samples. Я думаю чтобы достоверно разобратся надо вызивать INS функцию из ins.m и посмотреть что получается, можно и попробовать сравнить с INS Toolbox/Аerospace Toolbox если её можно достать. Цитата Поделиться сообщением Ссылка на сообщение Поделиться на другие сайты Поделиться
wookX 0 3 мая, 2007 Опубликовано 3 мая, 2007 · Жалоба 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 попозже выложу модифицированный код, построенный полностью на перемножении матриц. Но результаты почему то уж очень отличаются. :( Цитата Поделиться сообщением Ссылка на сообщение Поделиться на другие сайты Поделиться
CodeWarrior1241 0 3 мая, 2007 Опубликовано 3 мая, 2007 · Жалоба Странно... я так никогда не вызывал функцию в матлабе, обычно просто ввожу её inline, и вставляю products в следущию функцию, типа ode45. Цитата Поделиться сообщением Ссылка на сообщение Поделиться на другие сайты Поделиться
wookX 0 7 мая, 2007 Опубликовано 7 мая, 2007 · Жалоба Странно... я так никогда не вызывал функцию в матлабе, обычно просто ввожу её inline, и вставляю products в следущию функцию, типа ode45. А как это? не могли бы бы модифицировать мой код вашим решением, чтоб один файл был, а не два :) Цитата Поделиться сообщением Ссылка на сообщение Поделиться на другие сайты Поделиться
CodeWarrior1241 0 8 мая, 2007 Опубликовано 8 мая, 2007 · Жалоба А как это? не могли бы бы модифицировать мой код вашим решением, чтоб один файл был, а не два :) Не, я не это имел в виду, файлов все равно было бы 2, но call для integrate function был бы другим, не через string 'integrate' . У меня получилось бы еще одна строка. Но интересно не это - алгоритм раборает когда Вы его тестировали? Цитата Поделиться сообщением Ссылка на сообщение Поделиться на другие сайты Поделиться
wookX 0 9 мая, 2007 Опубликовано 9 мая, 2007 · Жалоба У меня получилось бы еще одна строка. Но интересно не это - алгоритм раборает когда Вы его тестировали? Файл 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 V=dcm'*[y(5);y(6);y(7)]; Vn=V(1); Ve=V(2); Vd=V(3); %%% 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 w=[p1;q1;r1] - dcm*ww; p1=w(1); q1=w(2); r1=w(3); % Quaternions Normalization y(1: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) U=[ax1;ay1;az1]-[p1;q1;r1]*[y(5);y(6);y(7)]+g*[-sin(theta1);cos(theta1)*sin(phi1);cos(theta1)*sin(phi1)]; Udot=U(1); Vdot=U(2); Wdot=U(3); %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 А второй файл ins.m будет вот таким: %%%Load file with accelerations and angular rates load 'initial.mat'; g=9.81; tmin=1; tmax=20000; t1=0:0.01: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 % 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)))); [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,:); 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 Цитата Поделиться сообщением Ссылка на сообщение Поделиться на другие сайты Поделиться