clc;
N = 100;
w = 0.5;
q = 0.01;
r = 0.1;
%%This is the transition matrix
A = [cos(w) sin(w)/w;
-w*sin(w) cos(w)];
%%This is the process noise covariance
Q = [0.5*q*(w-cos(w)*sin(w))/w^3 0.5*q*sin(w)^2/w^2;
0.5*q*sin(w)^2/w^2 0.5*q*(w+cos(w)*sin(w))/w];
% This is the true initial value
x0 = [0;0.1];
X = zeros(2,N); % The true signal
Y = zeros(1,N); % Measurements
T = 1:N; % Time
x = x0;
for i=1:N %kalman predict x(i) = A*x P(i) = A * P * A' + Q; end
