project3.m

N = 25;
theta = pi / 12;
A = [cos(theta) -sin(theta); sin(theta) cos(theta)];

R1 = [0 0; 0 0]; % process uncertainty %%
R2 = [1 0; 0 1];  % measurement uncertainty
C = eye(2);
Pm(:,:,1) = 1e5*eye(2); %P(N|N-1) %%
% kalman gain and estimate uncertainty
for k=1: N
    Kf(:,:, k) = Pm(:, :, k) * C' * (C * Pm(:, :, k) * C' + R2) ^ (-1);
    K(:,:, k) = A * Kf(:,:, k);
    P(:, :, k) = Pm(:, :, k) - Pm(:, :, k) * C' * (C * Pm(:, :, k) * C' + R2) ^ (-1) * C * Pm(:, :, k);
    Pm(:, :, k+1) = A * Pm(:, :, k)*A' - K(:,:, k)*(C*Pm(:, :,k)*C' + R2)*K(:,:, k)' + R1;
end
% w = random('normal', 0, 1, 1, N);
v = random('normal', 0, 1, 2, N);
x(:, 1) = [10; 0];
xhm(:, 1) = [10; 0];%%
% put measurement to y
y(1,1) = 7.1165;
y(1,2) = 9.6022;
y(1,3)=8.9144;
y(1,4)=9.2717;
y(1,5)=6.3400;
y(1,6)=4.0484;
y(1,7)=0.3411;
y(1,8)=-0.6784;
y(1,9)=-5.7726;
y(1,10)=-5.4925;
y(1,11)=-9.4523;
y(1,12)=-9.7232;
y(1,13)=-9.5054;
y(1,14)=-9.7908;
y(1,15)=-7.7300;
y(1,16)=-5.9779;
y(1,17)=-4.5535;
y(1,18)=-1.5042;
y(1,19)=-0.7044;
y(1,20)=3.2406;
y(1,21)=8.3029;
y(1,22)=6.1925;
y(1,23)=9.1178;
y(1,24)=9.0904;
y(1,25) = 9.0662;
y(2,1) = 0.000;
y(2,2) = 3.1398;
y(2,3)=6.3739;
y(2,4)=9.5877;
y(2,5)=10.1450;
y(2,6)=10.1919;
y(2,7)=9.0683;
y(2,8)=10.2254;
y(2,9)=7.5799;
y(2,10)=7.7231;
y(2,11)=5.4721;
y(2,12)=3.3990;
y(2,13)=0.9172;
y(2,14)=-1.3551;
y(2,15)=-5.2708;
y(2,16)-9.7011;
y(2,17)=-9.4256;
y(2,18)=-9.3053;
y(2,19)=-9.3815;
y(2,20)=-9.8822;
y(2,21)=-8.1876;
y(2,22)=-8.7501;
y(2,23)=-4.5653;
y(2,24)=-1.9179;
y(2,25)=-1.0000;
for k=1: N
    % x(:, k+1) = A*x(:, k) + [1; 1]*w(:, k);
    x(:, k+1) = A*x(:, k);
    xh(:, k) = xhm(:, k) + Kf(:,:, k)*(y(:,k)-C*xhm(:,k));
    xhm(:, k+1) = A*xhm(:, k) + K(:,:, k)*(y(:, k)-C*xhm(:, k));
end

% biases:
num = 0;
for k = 1: N
    % num = num + 10 * cos(2 * pi * (k-1) / (N-1)) - xh(1, k);
    num = num + x(1,k) - xh(1,k);
end
y_bias = num / N;

num = 0;
for k = 1: N
    num = num + x(2,k) - xh(2,k);
end
z_bias = num / N;

% variance:
num = 0;
for k = 1: N
    num = num + (x(1,k) - xh(1,k))^2;
end
y_variance = num / N;

num = 0;
for k = 1: N
    num = num + (x(2,k) - xh(2,k))^2;
end
z_variance = num / N;