Dragi forumaši pozdrav!
Imam mali problem s Matlabom, točnije sa izradom svoga programa. Budući da baš nisam vješti programer (točnije tek počinjem), nastao mi je mali problem u kojem, kao kolegi iz gornjeg posta, Matlab pokušava provarit ovo što sam napravio ali ne uspijeva.
Glavni problem mi je u tome što pokušavam izvršiti računanja za prvu vrijednost matrice,a potom da mi to isto radi za sve ostale vrijednosti...
Zalijepit ću vam dio koda koji me muči, pa se nadam da će se naći neka pametna glava koja će to znati rješiti. Ako i ne, hvala svima na trudu za čitanjem.
Ovo mi je radna verzija, tako da svaki komentar mi može biti od pomoći.
i = 1;
% Simulacija linearnog sustava (pravo stanje)
x = [xyz(i,1); 3.2250]; % početni vektor stanja
%(za os X: 4287402.35754, os Y: 1217339.53642, os Z: 4547431.00253,
%brzina: 3.2250)
% Simulacija mjerenja opterećenog šumom (pravo stanje)
Sum_mjerenja = sum_mjerenja * randn;
z = H * x + Sum_mjerenja;
% Procjena stanja u datom trenutku
x_potez = x; % početni vektor procjene stanja
% Izračun inovacije (rezidual mjerenja)
In = z - H * x_potez;
% Kovarijanca inovacije
S = R + H * P * H';
% Kalmanovo pojačanje
W = P * H' / S;
% Ažurirana procjena stanja
x_potez = x_potez + W * In;
% Ažurirana kovarijanca stanja
P = P - W * S * W';
for i = 2 : 1 : max(size(xyz))
for t = 0 : dt : trajanje
if xyz(i,1) == xyz(i-1,1)
% Simulacija linearnog sustava (pravo stanje)
Sum_procesa = sum_ubrzanja * [(dt^2/2)*randn; dt*randn];
x = F * x + G * u(i) + Sum_procesa;
% Simulacija mjerenja opterećenog šumom (pravo stanje)
Sum_mjerenja = sum_mjerenja * randn;
z = H * x + Sum_mjerenja;
% Procjena stanja u datom trenutku
x_potez = F * x_potez + G * u(i);
% Izračun inovacije (rezidual mjerenja)
In = z - H * x_potez;
% Kovarijanca inovacije
S = R + H * P * H';
% Kalmanovo pojačanje
W = P * H' / S;
% Ažurirana procjena stanja
x_potez = x_potez + W * In;
% Ažurirana kovarijanca stanja
P = P - W * S * W';
else
% Simulacija linearnog sustava (pravo stanje)
x = [xyz(i,1); Vgps(i)];
% Simulacija mjerenja opterećenog šumom (pravo stanje)
Sum_mjerenja = sum_mjerenja * randn;
z = H * x + Sum_mjerenja;
% Procjena stanja u datom trenutku
x_potez = x;
% Izračun inovacije (rezidual mjerenja)
In = z - H * x_potez;
% Kovarijanca inovacije
S = R + H * P * H';
% Kalmanovo pojačanje
W = P * H' / S;
% Ažurirana procjena stanja
x_potez = x_potez + W * In;
% Ažurirana kovarijanca stanja
P = P - W * S * W';
end
end
% Spremanje vektorskih parametara za kasnije crtanje
poz(i) = x(1);
poz_potez(i) = x_potez(1);
poz_mjerenja(i) = z;
brz(i) = x(2);
brz_potez(i) = x_potez(2);
i = i + 1;
end