Комментированный текст программы в MATLAB

% matlab kalman filter

% source code has been simplificated

format short e

clear posf a f_x f_y f_z ex ey ez stx sty stz pos_true pos_ins gps_pos

gps_off=140;

gps_on=180;

file=fopen('insposbb.dat', 'r');

tmp=fscanf(file, '%f ');

fclose(file);

[rows, cols]=size(tmp);

rows=rows/10;

for i=1: rows,

pos_true(i, 1: 4)= tmp(10*(i-1)+1: 10*(i-1)+4)';

pos_ins(i, 1: 3) = tmp(10*(i-1)+5: 10*(i-1)+7)';

gps_pos(i, 1: 3) = tmp(10*(i-1)+8: 10*(i-1)+10)';

end;

for i=2: rows,

if (pos_true(i, 4)==int32(pos_true(i, 4)));

else

gps_pos(i, 1: 3) = gps_pos(i-1, 1: 3) +pos_ins(i, 1: 3)-pos_ins(i-1, 1: 3); %надо ли это?

end;

end;

%----------------------------------------------------------------

plot3(pos_true(:, 1), pos_true(:, 2), pos_true(:, 3),...

gps_pos(:, 1), gps_pos(:, 2), gps_pos(:, 3),...

pos_ins(:, 1), pos_ins(:, 2), pos_ins(:, 3));

legend('True', 'GPS', 'INS');

title('Input trajectoty')

ylabel('East(m)')

xlabel('North(m)')

zlabel('Up(m)')

grid on

pause

close

%-----------------------------------------------------------------------

er1(1: rows, 1: 3) =pos_true(1: rows, 1: 3)-pos_ins(1: rows, 1: 3);

er2(1: rows, 1: 3) =pos_true(1: rows, 1: 3)-gps_pos(1: rows, 1: 3);

for i=1: rows,

er1n(i)= sqrt(er1(i, 1)*er1(i, 1)+er1(i, 2)*er1(i, 2)+er1(i, 3)*er1(i, 3));

er2n(i)= sqrt(er2(i, 1)*er2(i, 1)+er2(i, 2)*er2(i, 2)+er2(i, 3)*er2(i, 3));

end

plot(pos_true(1: 80, 4), er2n(1: 80));

title('GPS Errors');

xlabel('Time (sec)');

ylabel('Error (m)');

pause

close

plot(pos_true(1: 80, 4), er1n(1: 80));

title('INS Errors');

xlabel('Time (sec)');

ylabel('Error (m)');

pause

close

%-----------------------------------------------------------------------

[rows, cols]=size(pos_ins);

muu=1;

%ZADANIYE PARAMETROV ALGORITMA

L=25; prec_vist=0.1; prec=0.1;

%ZADANIYE RAZMERNOSTEY MASSIVOV FILTRA

w=zeros(L+1, 3); u=zeros(L+1, 3); e=zeros(rows, 3); P1=zeros(L+1, L+1); II_1=zeros(L+1, L+1); K1=zeros(L+1, 1);

P2=zeros(L+1, L+1); II_2=zeros(L+1, L+1); K2=zeros(L+1, 1); P3=zeros(L+1, L+1); II_3=zeros(L+1, L+1); K3=zeros(L+1, 1);

ww=zeros(L+1, 1); posf=zeros(rows, 3); posf_res=zeros(rows, 3);

%ZADANIYE PERVIH L ZNACHENIY FILTROVANNOGO SIGNALA

posf(1: L, 1: 3)=pos_ins(1: L, 1: 3); posf_res(1: L, 1: 3)=pos_ins(1: L, 1: 3);

%ZADANIYE NACHALNIH ZHACHENIY MASSIVOV FILTRA

u_ins=zeros(L+1, 3); u_gps=zeros(L+1, 3); ut=zeros(L+1, 1); u_ins_inv=zeros(L+1, 3); u_gps_inv=zeros(L+1, 3); ut_inv=zeros(L+1, 1);

u_ins_inv_short=zeros(L, 3); u_gps_inv_short=zeros(L, 3); ut_inv_short=zeros(L, 1);

for j = 1: L+1 u_ins(j, 1: 3)=pos_ins(L+1-j+1, 1: 3); u_gps(j, 1: 3)=gps_pos(L+1-j+1, 1: 3); ut(j)=pos_true(L+1-j+1, 4); end

for j = 1: L+1 u_ins_inv(j, 1: 3)=u_ins(L+1-j+1, 1: 3); u_gps_inv(j, 1: 3)=u_gps(L+1-j+1, 1: 3); ut_inv(j)=ut(L+1-j+1); end

for j = 1: L u_posf(j, 1: 3)=posf(L-j+1, 1: 3); end

for j = 1: L u_posf_inv(j, 1: 3)=u_posf(L-j+1, 1: 3); end

for j = 1: L+1 w(j, 1: 3) = 1/(L+1); u(j, 1: 3)=pos_ins(L+1-j+1, 1: 3); ww(j, 1: 3)=0; end

%VICHISLENIYE NACHALNIH PARAMETROV FILTRA, OPTIMALNIH DLYA DANNOGO SIGNALA (PO

%REZULTATAM OBRABOTKI PERVIH L ZNACHENIY)

C1=100/std(u_gps(:, 1))^2; C2=100/std(u_gps(:, 2))^2; C3=100/std(u_gps(:, 3))^2;

%ZADANIYE NACHALNIH ZHACHENIY OBRATNOY KORRELYATSIONNOY MATRITSI " P" FILTRA

for i = 1: L+1 P1(i, i)=C1; P2(i, i)=C2; P3(i, i)=C3; end

h1 = 1 + u(:, 1)'*P1*u(:, 1); h2 = 1 + u(:, 2)'*P2*u(:, 2); h3 = 1 + u(:, 3)'*P3*u(:, 3);

for i = 1: L+1 II_1(i, i)=1/h1; II_2(i, i)=1/h2; II_3(i, i)=1/h3; end

%VKLUCHENIYE ALGORITMA

warning off MATLAB: polyfit: RepeatedPointsOrRescale;

for k=L+1: rows-1

if((pos_true(k, 4)> gps_off) & (pos_true(k, 4)< gps_on))

%VICHISLENIYA PRI OTSUTSTVII GPS

for j = 1: 3

if(abs((pos_ins(k-1, j)-gps_pos(k-1, j))/pos_ins(k-1, j)) < prec)

uu=u_gps_inv_short(:, j);

fi = polyfit(ut_inv_short, log(uu), 2);

uf = exp(1).^polyval(fi, ut_inv_short);

gps_pos(k, j)=interp1(ut_inv_short, uf, ut_inv(L+1), 'linear', 'extrap');

% gps_pos(k, j)=interp1(ut_inv_short, u_gps_inv_short(:, j), ut_inv(L+1), 'linear', 'extrap');

else

gps_pos(k, j)= (posf_res(k-1, j) + gps_pos(k-1, j))/2;

end

end

%KONETS VICHISLENIY PRI OTSUTSTVII GPS

else

%PROTSEDURA " VISTAVKI" (PERENORMIROVKI) SIGNALA INS PRI EGO ZNACHITELNOM

%OTKLONENII OT GPS

for j = 1: 3

if(abs((pos_ins(k, j)-gps_pos(k, j))/abs(pos_ins(k, j))) > prec_vist)

delta=pos_ins(k, j)-gps_pos(k, j);

for jj=k: rows

pos_ins(jj, j)=pos_ins(jj, j)-delta;

end

else

end

end

end

%FILTR KALMANA

posf(k, 1)=u(:, 1)'*ww(:, 1); posf(k, 2)=u(:, 2)'*ww(:, 2); posf(k, 3)=u(:, 3)'*ww(:, 3);

e(k, 1: 3)=gps_pos(k, 1: 3) - posf(k, 1: 3);

h1 = 1 + u(:, 1)'*P1*u(:, 1); h2 = 1 + u(:, 2)'*P2*u(:, 2); h3 = 1 + u(:, 3)'*P3*u(:, 3);

for i = 1: L+1 II_1(i, i)=1/h1; II_2(i, i)=1/h2; II_3(i, i)=1/h3; end

K1 = II_1*P1*u(:, 1); K2 = II_2*P2*u(:, 2); K3 = II_3*P3*u(:, 3);

for i=1: L+1

ww(i, 1) = ww(i, 1) + K1(i)*e(k, 1); ww(i, 2) = ww(i, 2) + K2(i)*e(k, 2); ww(i, 3) = ww(i, 3) + K3(i)*e(k, 3);

end

P1 = P1 - K1*u(:, 1)'*P1; P2 = P2 - K2*u(:, 2)'*P2; P3 = P3 - K3*u(:, 3)'*P3;

%OBNOVLENIYE VSPOMOGATELNIH MASSIVOV

for j=1: L+1 u_ins(j, 1: 3)=pos_ins(k+1-j+1, 1: 3); u_gps(j, 1: 3)=gps_pos(k+1-j+1, 1: 3); ut(j)=pos_true(k+1-j+1, 4); end

for j=1: L+1 u_ins_inv(j, 1: 3)=u_ins(L+1-j+1, 1: 3); u_gps_inv(j, 1: 3)=u_gps(L+1-j+1, 1: 3); ut_inv(j)=ut(L+1-j+1); end

for j=1: L u_ins_inv_short(j, 1: 3)=u_ins_inv(j, 1: 3); u_gps_inv_short(j, 1: 3)=u_gps_inv(j, 1: 3); ut_inv_short(j)=ut_inv(j); end

for j=1: L u_posf(j, 1: 3)=posf(k-j+1, 1: 3); end

for j=1: L u_posf_inv(j, 1: 3)=u_posf(L-j+1, 1: 3); end

%VICHILENIYE KHARASTERISTIK SIGNALA GPS (POSLEDNIH L ZNACHENIY)

for j = 1: 3

m = mean(u_gps(:, j)); d = std (u_gps(:, j)); delta=sign(gps_pos(k, j)-m); posf_res(k, j)=gps_pos(k, j)-delta*d*muu;

% posf(k, j)=(posf_res(k, j)+posf(k, j))/2; posf(k, j)=(gps_pos(k, j)+posf(k, j))/2;

end

%OBNOVLENIYE LINII ZADERJKI

for i=1: L+1

u(i, 1: 3) = pos_ins(k+1-i+1, 1: 3);

end

%VSPOMOGATELNIY FILTR DLYA SGLAJIVANIYA VOZMOJNIH " POLUSOV"

for j = 1: 3

m = mean(u_gps(:, j));

if(abs((posf(k, j)-m)/m) > prec)

posf(k, j) = posf_res(k, j);

end

end

%KONETS ALGORITMA

end

posf(rows, 1: 3)=posf(rows-1, 1: 3);

%for i=2: rows,

% if (pos_true(i, 4)> gps_off) & (pos_true(i, 4)< gps_on)

% gps_pos(i, 1: 3)=[0 0 0];

% end;

%end;

subplot(311);

plot(pos_true(:, 4), pos_true(:, 1), pos_true(:, 4), pos_ins(:, 1), pos_true(:, 4), posf(:, 1), pos_true(:, 4), gps_pos(:, 1));

title('Position: X Y Z');

legend('True', 'INS', 'Filtered', 'GPS');

subplot(312);

plot(pos_true(:, 4), pos_true(:, 2), pos_true(:, 4), pos_ins(:, 2), pos_true(:, 4), posf(:, 2), pos_true(:, 4), gps_pos(:, 2));

subplot(313);

plot(pos_true(:, 4), pos_true(:, 3), pos_true(:, 4), pos_ins(:, 3), pos_true(:, 4), posf(:, 3), pos_true(:, 4), gps_pos(:, 3));

pause

close

%-------------------------------------------------------------------

er1(1: rows, 1: 3) =pos_true(1: rows, 1: 3)-pos_ins(1: rows, 1: 3);

er2(1: rows, 1: 3) =pos_true(1: rows, 1: 3)-gps_pos(1: rows, 1: 3);

erf(1: rows, 1: 3) =pos_true(1: rows, 1: 3)-posf(1: rows, 1: 3);

for i=2: rows,

if (pos_true(i, 4)> gps_off) & (pos_true(i, 4)< gps_on)

er2(i, 1: 3)=[0 0 0];

end;

end;

for i=1: rows,

er1n(i)= sqrt(er1(i, 1)*er1(i, 1)+er1(i, 2)*er1(i, 2)+er1(i, 3)*er1(i, 3));

er2n(i)= sqrt(er2(i, 1)*er2(i, 1)+er2(i, 2)*er2(i, 2)+er2(i, 3)*er2(i, 3));

erfn(i)= sqrt(erf(i, 1)*erf(i, 1)+erf(i, 2)*erf(i, 2)+erf(i, 3)*erf(i, 3));

end;

subplot(311)

plot(pos_true(:, 4), er1(:, 1), pos_true(:, 4), er2(:, 1), pos_true(:, 4), erf(:, 1));

title('Errors');

legend('INS', 'GPS', 'Filtering');

subplot(312)

plot(pos_true(:, 4), er1(:, 2), pos_true(:, 4), er2(:, 2), pos_true(:, 4), erf(:, 2));

subplot(313)

plot(pos_true(:, 4), er1(:, 3), pos_true(:, 4), er2(:, 3), pos_true(:, 4), erf(:, 3));

xlabel('Time (sec)');

pause

close

plot(pos_true(:, 4), er1n(:), pos_true(:, 4), er2n(:), pos_true(:, 4), erfn(:));

title('Trajectoty Errors');

legend('INS', 'GPS', 'Filtering');

pause

close

%---------------------------------------------------------------

Пример файла inposb.dat. (первые несколько строк), из которого производится считывание (по столбцам) всех значений сигналов

0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -5.256897 -11.935277 -7.555930

5.047140 6.014947 6.539573 0.100000 5.051447 6.014832 6.553451 -5.256897 -11.935277 -7.555930

10.340481 12.323306 13.302540 0.200000 10.351656 12.313794 13.333108 -5.256897 -11.935277 -7.555930

15.880765 18.925959 20.287519 0.300000 15.897750 18.890825 20.329712 -5.256897 -11.935277 -7.555930

21.669404 25.824590 27.491851 0.400000 21.691040 25.760259 27.539151 -5.256897 -11.935277 -7.555930

27.707724 33.020780 34.913010 0.500000 27.732178 32.924199 34.936767 -5.256897 -11.935277 -7.555930

33.996998 40.516045 42.548537 0.600000 34.019838 40.379892 42.518181 -5.256897 -11.935277 -7.555930

40.538453 48.311847 50.396032 0.700000 40.561536 48.125663 50.295706 -5.256897 -11.935277 -7.555930

47.333270 56.409594 58.453155 0.800000 47.364391 56.172686 58.292680 -5.256897 -11.935277 -7.555930

54.382587 64.810644 66.717622 0.900000 54.425978 64.531032 66.520095 -5.256897 -11.935277 -7.555930

61.687505 73.516305 75.187202 1.000000 61.749413 73.188564 74.953589 43.853184 89.305123 73.126352

69.249085 82.527846 83.859715 1.100000 69.331244 82.153972 83.585484 43.853184 89.305123 73.126352

77.068356 91.846490 92.733029 1.200000 77.173205 91.440795 92.423211 43.853184 89.305123 73.126352

85.146311 101.473422 101.805057 1.300000 85.281459 101.045205 101.463869 43.853184 89.305123 73.126352