% diskretointiväli
dt = 1;
% simuloinnin loppuaika
T = 200;
% aikavektori
vecT = 0:dt:T;
% ajanhetkien lukumäätä
nt = numel(vecT);

% varataan tila simulaation tuloksille etukäteen
% tila
x = zeros(2, nt);
% mittaukset
y = zeros(1, nt);
% nopeuden brute force-estimaatti
vbf = zeros(1, nt);
% tilaestimaatti
xest = zeros(2, nt);
% Kalman-vahvistus
K = zeros(2, nt);


% auton tilaesityksen matriisit (B ja D tyhjiä, koska ei ohjausta)
A = [1, dt; 0, 1];
% ulostulona auton paikka
C = [1, 0];

% auton alkutila
x(:, 1) = [0; 5];

% alussa estimaattina auton todellinen paikka
xest(:, 1) = x(:, 1);

% prosessikohinan kovarianssimatriisi
R1 = [5, 10; 10, 20];

% mittauskohinan varianssi
R2 = 2500;

% tilan estimointivirheen kovarianssimatriisin alkuarvo
P = 1000*eye(2);

% Animoidaanko tulokset?
bAnimate = true;

% simulointisilmukka
for n = 2:nt
    
    % "todellisuus"
    a = ???; % kokeile erilaisia!
    x(1, n) = x(1, n - 1) + dt*x(2, n - 1) + 0.5*a*dt^2;
    x(2, n) = x(2, n - 1) + dt*a;
    
    % mittaus
    y(n) = x(1, n) + randn*sqrt(R2);
    
    % nopeuden brute-force-estimaatti (TÄYTÄ)
    vbf(n) = ???;
    
    
    % KALMAN-SUODIN (TÄYTÄ KAIKKI)
    % Ennakointi ja estimointi. Vain Kalman-vahvistus, tilaestimaatti ja
    % estimointivirheen kovarianssimatriisi tallennetaan, mutta laskuissa
    % saa käyttää muitakin muuttujia!
    ??? = ???;
    K(:, n) = ???; % Kalman-vahvistus ajanhetkellä n
    xest(:, n) = ???; % tilaestimaatti ajanhetkellä n
    P = ???; % viimeisin estimointivirheen kovarianssi
    
end

% piirretään tulokset
% auton paikka
figure;
subplot(2, 1, 1)
axis([0, T, min([min(xest(1, :)), min(x(1, :))]) - 1,...
    max([max(xest(1, :)), max(x(1, :))]) + 1]);
title('Auton paikka', 'FontSize', 12)
xlabel('aika [s]');
ylabel('paikka [m]')
hold on

% auton nopeus
subplot(2, 1, 2)
axis([0, T, min([min(xest(2, :)), min(x(2, :))]) - 1,...
    max([max(xest(2, :)), max(x(2, :))]) + 1]);
title('Auton nopeus', 'FontSize', 12)
xlabel('aika [s]');
ylabel('nopeus [m/s]')
hold on

% jos bAnimate, animoidaan tulokset
if bAnimate
    
    tCompression = 100; % ajan nopeutuskerroin
    for ii = 1:nt
        
        if ii > 1
            delete(hX);
            delete(hY);
            delete(hXEst);
            delete(hVTrue);
            delete(hVBF);
            delete(hVEst);
        end
        
        subplot(2, 1, 1);
        hX = plot(vecT(1:ii), x(1, 1:ii), '-b');
        hY = plot(vecT(1:ii), y(1, 1:ii), 'rx');
        hXEst = plot(vecT(1:ii), xest(1, 1:ii), '-m');
        
        subplot(2, 1, 2)
        hVTrue = plot(vecT(1:ii), x(2, 1:ii), '-b');
        hVBF = plot(vecT(1:ii), vbf(1:ii), 'rx');
        hVEst = plot(vecT(1:ii), xest(2, 1:ii), '-m');
        
        
        pause(dt/tCompression);
        
    end
else
    
    subplot(2, 1, 1);
    hX = plot(vecT, x(1, :), '-b');
    plot(vecT, y(1, :), 'rx');
    hXEst = plot(vecT, xest(1, :), '-r');
    
    
    subplot(2, 1, 2)
    hVTrue = plot(vecT, x(2, :), '-b');
    hVBF = plot(vecT, vbf, 'gx');
    hVEst = plot(vecT, xest(2,:), '-r');
    
    
end

subplot(2, 1, 1)
legend('todellisuus', 'mittaus', 'KF-estimaatti');
subplot(2, 1, 2)
legend('todellisuus', 'brute force-estimaatti', 'KF-estimaatti');

% lasketaan virheet
rmse_pos_meas = sqrt(mean((y-x(1,:)).^2)) % paikan mittauksen rms-virhe
rmse_pos_kf = sqrt(mean((xest(1,:)-x(1,:)).^2)) % paikan kf-estimaatin rms-virhe

rmse_vel_bruteforce = sqrt(mean((vbf-x(2,:)).^2)) % nopeuden brute force-estimaatin rms-virhe
rmse_vel_kf = sqrt(mean((xest(2,:)-x(2,:)).^2)) % nopeuden kf-estimaatin rms-virhe