wookX

И еще вот недостающая информация. The midpoint acceleration error in terms of the unknown accelerometer parameters is given by: dbm=bm*psi+ba+ha*bm+(bm^2)*(FI1-FX1)+db; ba - (3x1) vector of unknown accelerometer biases, normalized to the magnitude of gravity ha - (3x3) matrix |S1 d12 d13| ha= |0 S2 d23| |0 0 S3 | S1,S2,S3 - unknown accelerometer scale factor errors. d12,d13,d23 - unknown accelerometer axes nonorthogonalities (misalignments) db - represents other error terms, some of which are observable; for reason of practicality in our example they are not estimated, only compensated with factory calibrated values. FI1 - (3x1) unknown acceleration-squared nonlinearity for acceleration along the accelerometer input axis FX1 - (3x1) unknown acceleration-squared nonlinearity for acceleration normal to accelerometer input axis bm - is a three vector (b1, b2, b3)' of midpoint components of acceleration in platform coordinates |b1^2 0 0 | bm^2= |0 b2^2 0 | |0 0 b3^2| Как мне из всего, что здесь написано сворганить код для фильтра?

Файл 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

А как это? не могли бы бы модифицировать мой код вашим решением, чтоб один файл был, а не два :)

Пока запустить не удалось :(( Нашлось несколько ошибочек. Должно быть вот так: % Accelerometer state vector Xa=[ba' S1 d12 S2 d13 d23 S3 (FX1-FI1)']'; и вот так вот Ha=[b1,b2,b3,b1^2,b1*b2,b1*b3,b2^2,b2*b3,b3^2,(1-b1^2)*b1,(1-b2^2)*b2,(1-b3^2)*b3]'; %Difference equation for the accelerometers Xa(i)=Fa*Xa(i-1)+wa(i-1) %Accelerometer observation equation Za(i)=Ha*Xa(i)+Va(i) А как UKF построить?

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 попозже выложу модифицированный код, построенный полностью на перемножении матриц. Но результаты почему то уж очень отличаются. :(

Имеется алгоритм Бесплатформенной Инерциальной Навигационной Системы. Состоит из двух файлов. Если кто в этом разбирается, не могли бы подсказать, алгоритм функционирует или всеже нет? Файл первый: 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

Алгоритм получается следующий: % Accelerometer state vector Xa=[ba S1 d12 S2 d13 d23 S3 (FX1-FI1)]'; % State transition matrix Fa=I % Unknown accelerometer biases, normalized to the magnitude of gravity ba=[ba1; ba2; ba3]; % Unknown acceleration-squared nonlinearity for acceleration along the acelerometer input axis FI1=[FI11; FI12; FI13]; % Unknown acceleration-squared nonlinearity for acceleration normal to the acelerometer input axis FX1=[FX11; FX12; FX13]; % Unknown accelerometer scale factor errors S1 S2 S3 % Unknown accelerometer axes nonorthogonalities d12 d13 d23 % Midpoint components of acceleration in platform coordinates bm^2=[b1^2 0 0; 0 b2^2 0; 0 0 b3^2]; bm=[b1 b2 b3]'; Ha=[b1,b3,b1^2,b1,b2,b1,b3,b2^2,b2,b3,b3^2,(1-b1^2)*b1,(1-b2^2)*b2,(1-b3^2)*b3]; %%%%%%%%%%%%%% % EKF module % %%%%%%%%%%%%%% % Input initial parameters Rk, xa, Za, Ha, FY % xa=[0 0 0 0 0 0 0 0 0 0 0 0] Id=[1 0 0 0 0 0 0 0 0 0 0 0; 0 1 0 0 0 0 0 0 0 0 0 0; 0 0 1 0 0 0 0 0 0 0 0 0; 0 0 0 1 0 0 0 0 0 0 0 0; 0 0 0 0 1 0 0 0 0 0 0 0; 0 0 0 0 0 1 0 0 0 0 0 0; 0 0 0 0 0 0 1 0 0 0 0 0; 0 0 0 0 0 0 0 1 0 0 0 0; 0 0 0 0 0 0 0 0 1 0 0 0; 0 0 0 0 0 0 0 0 0 1 0 0; 0 0 0 0 0 0 0 0 0 0 1 0; 0 0 0 0 0 0 0 0 0 0 0 1]; % Computing Kalman gain Kg = Pk*Ha'*inv(Ha*Pk*Ha'+Rk); %Kg = (Pk*Hk')/(Hk*Pk*Hk'+Rk); % Computing the predicted measurment za = Ha*xa; % Conditioning the predicted estimate on the measurement Xa = xa + Kg*(Za - za); % Computing a posteriori covariance matrix for update estimate P2k= (Id-Kg*Ha)*Pk; % Computing the a priori covariance matrix ahead for next time step Pk = FY*P2k*FY' + Qk; % Computing the predicted state estimate xaf = (FY*Xa)'; Ничего не забыл? кто разбирается может подскажет а? Нигде не напортачил???

Всем большое спасибо. Получилось. Правда наверно не оптимально, но всетаки :) clear s = serial('COM2'); set(s,'BaudRate',4800); fopen(s); for i=1:10%00 data1=fgetl(s); ID = sscanf(data1,'%6c',1); if ID == '$GPRMC' X=strread(data1(8:end),'%s', 'delimiter', ','); UTC=char(X(1)); Time(i)=str2num(UTC(1:2))*3600; Time(i)=Time(i)+str2num(UTC(3:4))*60; Time(i)=Time(i)+str2num(UTC(5:end)); Latitude=char(X(3)); Lat(i)=str2num(Latitude(1:2)); Lat(i)=Lat(i)+str2num(Latitude(3:end))/60; NS=char(X(4)); if (NS == 'S')|(NS == 's') Lat(i)=-Lat(i); end Longitude=char(X(5)); Long(i)=str2num(Longitude(1:3)); Long(i)=Long(i)+str2num(Longitude(4:end))/60; EW=char(X(6)); if (EW == 'W')|(EW == 'w') Long(i)=360-Long(i); end Speed=char(X(7)); Vt(i)=str2num(Speed); Course=char(X(8)); Psi(i)=str2num(Course); end end fclose(s); delete(s);

Я не понимаю, почему, когда вводишь: A='ABC' Ответ: A= ABC Без кавызек??? А когда я считал он сам поставил эти ковычки? И если считать без кавычек, то строки будут выглядеть так???? Time(i)=str2num(UTC(2:3)); Time(i)=Time(i)+str2num(UTC(4:5))/60; Time(i)=Time(i)+str2num(UTC(6:end-1))/3600; % Не уверен можно ли так записать --> end-1

День добрый. У меня возникла проблема. Подключил ГПС приемник к компу. Открыл в Матлаб СОМ порт. Подключился к ГПС приемнику. Считал NMEA информацию. А как теперь ее разобрать? Чтоб выделить Широту, долготу, время? Листинг программы следующй: %Create a serial port object. obj1 = instrfind('Type', 'serial', 'Port', 'COM3', 'Tag', ''); %Create the serial port object if it does not exist %otherwise use the object that was found. if isempty(obj1) obj1 = serial('COM3'); else fclose(obj1); obj1 = obj1(1) end %Connect to instrument object, obj1. fopen(obj1); %Read NMEA sequencies for i=1:100 data1 = fscanf(obj1); ID=sscanf(data1,'%6c',1) If ID == '$GPRMC' [uTC, Status, Latitude, NS, Longitude, EW, Speed, Course, Date, Magnetic, Mode]=strread(data1(8:end),'%s %s %s %s %s %s %s %s %s %s %s', 'delimiter',',') Time(i)=str2num(UTC(1:2)); Time(i)=Time(i)+str2num(UTC(3:4))/60; Time(i)=Time(i)+str2num(UTC(5:end))/3600; %Time(i)=Time(i)+str2num(UTC(5:9))/3600; Lat(i)=str2num(Latitude(1:2)); Lat(i)=Lat(i)+str2num(Latitude(3:end))/60; %Lat(i)=Lat(i)+str2num(Latitude(3:10))/60; if (NS == 'S')|(NS == 's') Lat(i)=-Lat(i); end Long(i)=str2num(Longitude(1:3)); Long(i)=Long(i)+str2num(Longitude(4:end))/60; %Long(i)=Long(i)+str2num(Longitude(4:11))/60; if (EW == 'W')|(EW == 'w') Long(i)=360-Long(i); end Vt(i)=str2num(Speed); Psi(i)=str2num(Course); end %Disconnect from instrument object, obj1. fclose(obj1); %Clean up all objects. delete(obj1); Но вот не работает код :( После строки [uTC, Status, Latitude, NS, Longitude, EW, Speed, Course, Date, Magnetic, Mode]=strread(data1(8:end),'%s %s %s %s %s %s %s %s %s %s %s', 'delimiter',',') вылезает ошибка. Index Exceed Matrix Dimension Когда я открыл переменную Status, то там было следующее: UTC = '182524' С ковычками!!!! А str2num не воспринимает строковую переменную в данном виде! Может кто подскажет в чем проблема и как выделить информацию из NMEA???

Вот пожалуйста исходники от книги описанной выше. Может у кого нибудь есть исходники от книжки Global Positioning System, Inertial navigation and Integration? асколько я понял в моем случае надо воспользоваться Многомерным цифровым фильтром Калмана. Для этого мне нужно получить: 1) Модель сообщения 2) Модель Наблюдения 3) Априорные данные 4) Уравнение Фильтра Калмана 5) Коеффициент Усиления 6) Апостериорная Матрица Дисперсий 7) Начальные условия Но вот как это все применить для моего случая я не знаю :( Kalman_Filtering___Theory_and_Practice_using_MATLAB__2nd_Ed.zip

Погуглил я книжку GPS, Inertial Navigation and Integration. Вообщем книжка написана на профессионала в этом деле :( Я много от туда не понял :( Да она и без исходников :( Если бы еще файлы к ней были, может быть на примерах и понял что к чему. Хотя видел в Интернете GPS, Inertial Navigation and Integration. Second Edition. Оглавление отличается от того, что написано в оглавлении первого издания. ММожет в этой книжке и более понятно радовому пользователю понатно все расписано. Если у кого уже есть второйе изданийе, не поделитесь а? Плиззззз. П.С. Комы интересно могу выложить файлы исходников от книжки Kalman Filtering Theory And Practice Using Matlab - Second Edition.

И еще вот, я тут нашел. Вроде то что нужно, но не могли бы вы мне помочь с этим? :( А то я как то совсем ничего не понял :( 5.4.2.pdf Example_5.4.pdf

Три блока акселерометров ставлю, так как мне их три прислали :))) Думаю раз уж есть три источника одной о той же информации, то можно же как то улудшить измеряемую величину. Гироскопы тоже имеются. Так же три штуки - двухосевые. Драфт схемы установки находится в приложенном файле. Жду васших советов, рекомендаций и помощи. acct1.doc

А у Analog Devices можно заказать samples? А то я пробовал заказать - а они мне отвечяют - Contact Your Local Distributer. А Distributer сразу говорит что samples они предоставить не могут :(