MSI Exercise 5 Matlab code

Dateiinhalt

close all
clc

load 'data5.txt';

%% Weighted least squares

t      = data5(:,1);
p_meas = data5(:,2);
v_meas = data5(:,3);

N = length(t);

C2 = .1;                   
C3 = .6;
D = .4;

%v_X = (C1*D - C2) / C3 .* (1 - exp(-C3 * t)) + v_0 * exp(-C3 * t);
%p_X = 

% Ordinary least squares

Phi_p = [D/C3 * t + D / C3^2 * (exp(-C3 * t) - 1), ones(size(t)), (1-exp(-C3 * t))/C3];
y_p   = p_meas + C2/C3 * t + C2 / C3^2 * (exp(-C3 * t) - 1);

Phi_v = [D/C3 - D/C3 * exp(-C3*t), zeros(size(t)) exp(-C3*t)];
y_v   = v_meas + C2/C3 - C2/C3 * exp(-C3 * t);

Phi = [Phi_p ; Phi_v];
y   = [y_p   ; y_v];

theta_ols = inv(Phi' * Phi) * Phi' * y;

cov_noise = blkdiag(.1^2*eye(N), .05^2*eye(N));
cov_theta_ols = inv(Phi' *  Phi) * Phi' * cov_noise * (inv(Phi' *  Phi) * Phi')';

% Weighted least squares

W = inv(cov_noise);
theta_wls = inv(Phi' * W *  Phi) * Phi' * W * y;

% Covariance matrix of theta

cov_theta_wls = inv(Phi' * W *  Phi) * Phi' * W * cov_noise * (inv(Phi' * W *  Phi) * Phi' * W)';

theta_wls

%% Recursive least squares

theta_rec = zeros(3,1);
thetas = zeros(3,N);

% prior knowledge
Q = inv( 100 * eye(3));

for i = 1:N
   
    y_i = [y(i) ; y(N + i)];
    phi_i = [Phi(i,:)' , Phi(N + i,:)'];
    Q = Q + phi_i * phi_i';
    
    theta_rec = theta_rec + inv(Q)*(phi_i * ( y_i - phi_i' * theta_rec ));
    
    thetas(:,i) = theta_rec;
    
end

theta_rec

plot(thetas')