clear clc close all %% Model characteristics % x(t+1) = F * x(t) + v1(t) ---> F: State transition matrix, v1 = N(0,V1) : Process noise % y(t) = H * x(t) + v2(t) ---> H: Output matrix , v2 = N(0,V2) : Observation noise StateDim = 2; % Number of states ObsDim = 1; % Number of observations DeltaT = 0.01; % sampling time F = [1 DeltaT % motion model: 1D, constant velocity 0 1 ]; H = zeros(ObsDim,StateDim); H(:,1) = 1; % Matrix H N = 3000; % how many datapoints? X = zeros(StateDim,N); % State data buffer y = zeros(ObsDim,N); % Observation data buffer timestep = (0:N-1)*DeltaT; % sampling time instants xh_prior = zeros(StateDim,N); % buffer for the Kalman 1-step ahead predictor xh_filt = zeros(StateDim,N); % filterbuffer for the Kalman filter as yh = zeros(size(y)); % output estimation buffer e = zeros(size(yh)); % innovation buffer Pmat = zeros(StateDim, StateDim, N); % buffer for the P matrices K0mat = zeros(StateDim, ObsDim, N); % buffer for the Kalman filter gain Kmat = zeros(StateDim, ObsDim, N); % buffer for the Kalman predictor gain %% Generate Process Noise (Noise of the state equations) % V1 = [1 0.0;0.0 .05]; % Process Noise Covariance Matrix % Mu_PNoise = 0; % Process Noise mean % Std_PNoise = chol(V1); % Standard deviation of the process noise % PNoise = Std_PNoise * randn(StateDim,N) + Mu_PNoise*ones(StateDim,N); % Gaussian Process Noise % V1=zeros(2); PNoise = zeros(StateDim,N); %% Generate Observation Noise (Noise of the output equation0 V2 = 20; % Observation Noise Covariance Matrix Mu_ONoise = 0; % Observation Noise mean Std_ONoise = sqrt(V2); % Standard deviation of the observation noise ONoise = Std_ONoise * randn(ObsDim,N) + Mu_ONoise*ones(ObsDim,N); % Gaussian Observation Noise %% Initial values for the model X(:,1) = [1 2]'; % Initial state y(1) = H * X(:,1) + ONoise(:,1); % Initial observation %% Simulate states and observations % saving results into the buffers X and y for t = 1 : N-1 X(:,t+1) = F * X(:,t) + PNoise(:,t); % States y(:,t) = H * X(:,t) + ONoise(:,t); % Real Observations end y(:,N) = H * X(:,N) + ONoise(:,N); %% Kalman filtering... % initialization xh_prior(:,1) = X(:,1); %zeros(StateDim,1); % Initial state P1 = eye(StateDim); % Initial state covariance matrix Pmat(:,:,1) = P1; % KF as filter for t = 1 : N-1 yh(:,t) = H * xh_prior(:,t); % a priori output estimation e(t) = y(:,t) - yh(:,t); % innovation K0mat(:,:,t) = Pmat(:,:,t)*H'*inv(H*Pmat(:,:,t)*H'+V2); % actual Kalman gain Kmat(:,:,t) = F* K0mat(:,:,t); xh_prior(:,t+1) = F*xh_prior(:,t)+Kmat(:,:,t)*e(:,t); % state predictor Pmat(:,:,t+1) = [F-F*Kmat(:,:,t)*H]*Pmat(:,:,t)*[F-F*Kmat(:,:,t)*H]'+V1+F*Kmat(:,:,t)*V2*(F*Kmat(:,:,t))'; % updating the state error covariance matrix (DRE) xh_filt(:,t) = xh_prior(:,t)+K0mat(:,:,t)*e(:,t); % KF AS FILTER end % for yh(:,N) = H * xh_prior(:,N); % a priori output estimation e(N) = y(:,N) - yh(:,N); % innovation K0mat(:,:,N) = Pmat(:,:,N)*H'*inv(H*Pmat(:,:,N)*H'+V2); % actual Kalman gain xh_filt(:,N) = xh_prior(:,N)+K0mat(:,:,N)*e(:,N); % KF AS FILTER %% Plot the estimation results figure('Name','Observations'); plot(timestep,y,'-ob', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'b' ); xlabel('time [s]'); ylabel('position [m]'); hold on; plot(timestep,yh,'-dr', ... 'LineWidth',1.5, 'MarkerSize',8, 'MarkerFaceColor', 'r') grid on leg1 = legend('Real observation $y_1$',... '1-step-ahead pred. observation $\hat{y}_1$',... 'location', 'best'); set(leg1,'Interpreter','latex'); set(leg1,'FontSize',18); set(gca, 'FontSize',14); figure('Name','State vs estimation'); h1 = subplot(2,1,1); plot(timestep, X(1,:),'-ob', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'b'); xlabel('time [s]'); ylabel('x_1 -> position [m]'); hold on; plot(timestep, xh_filt(1,:),'-vr', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'r' ); grid on xleg1 = legend('Real $x_1$',... 'filtered estim. $\hat{x}_1$',... 'location', 'best'); set(xleg1,'Interpreter','latex'); set(xleg1,'FontSize',18); set(gca, 'FontSize',14); h2 = subplot(2,1,2); plot(timestep, X(2,:),'-ob', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'b'); xlabel('time [s]'); ylabel('x_2 -> speed [m/s]'); hold on; plot(timestep, xh_filt(2,:),'-vr', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'r' ); grid on xleg2 = legend('Real $x_2$',... 'filtered estim. $\hat{x}_2$',... 'location', 'best'); set(xleg2,'Interpreter','latex'); set(xleg2,'FontSize',18); set(gca, 'FontSize',14); linkaxes([h1 h2], 'x');