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 = 4; % Number of states ObsDim = 2; % Number of observations DeltaT = 0.01; % sampling time [s] % ------------------------------------------------ % motion model: 2D, constant velocity, rigid body % --------------- % state variables: % x_1 --> component along the x-axis of the body position [m] % x_2 --> component along the x-axis of the body speed [m/s] % x_3 --> component along the y-axis of the body position [m] % x_4 --> component along the y-axis of the body speed [m/s] % ---------------- % observations: % y_1 --> x_1 % y_2 --> x_3 % ------------------------------------------------- F = [1 DeltaT 0 0; % position update - component along the x axis 0 1 0 0; % speed update - component along the x axis 0 0 1 DeltaT; % position update - component along the y axis 0 0 0 1]; % speed update - component along the y axis H = zeros(ObsDim,StateDim); H(1,1) = 1; H(2,3) = 1; % Matrix H N = 300; % 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 = zeros(StateDim,N); % buffer for the Kalman 1-step ahead predictor yh = zeros(size(y)); % output estimation buffer e = zeros(size(yh)); % innovation buffer Pmat = zeros(StateDim, StateDim, N); % buffer for the P matrices Kmat = zeros(StateDim, ObsDim, N); % buffer for the Kalman gain %% Generate Process Noise (Noise of the state equations) % V1 = diag([0.01; 0.25;0.01; 0.25]);% Process Noise Covariance Matrix % --> 4 independent WGN stochastic processes 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 %} %{ % NO disturbances/uncertainties on state variables % V1=zeros(StateDim); PNoise = zeros(StateDim,N); % %} %% Generate Observation Noise (Noise of the output equation0 V2 = .9* [.1 0.01;0.01 0.1]; % Observation Noise Covariance Matrix % --> 2 WGN stochastic processes; NB the random variables are correlated Mu_ONoise = 0; % Observation Noise mean Std_ONoise = chol(V2); % Standard deviation of the observation noise ONoise = Std_ONoise * randn(ObsDim,N) + Mu_ONoise*ones(ObsDim,N); % Gaussian Observation Noise % ONoise = zeros(ObsDim,N); % V2=zeros(ObsDim); %% Initial values for the model xpos0 = 0.1; % [m] ypos0 = 1.25;% [m] xspeed0 = 0.0; % [m/s] yspeed0 = 0.0;% [m/s] P1 = diag([0.15;1.5;0.15;1.5]); % Initial state covariance matrix P1squared = chol(P1); X(:,1) = [xpos0; xspeed0;ypos0;yspeed0]; % 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); % Real Observations %% Kalman filtering... % initialization xh(:,1) = zeros(StateDim,1); % Initial state Pmat(:,:,1) = P1; % 1 step ahead predictor for t = 1 : N-1 yh(:,t) = H * xh(:,t); % a priori output estimation e(:,t) = y(:,t) - yh(:,t); % innovation Kmat(:,:,t) = F*Pmat(:,:,t)*H'*inv(H*Pmat(:,:,t)*H'+V2); % actual Kalman gain xh(:,t+1) = F*xh(:,t)+Kmat(:,:,t)*e(:,t); % state predictor Pmat(:,:,t+1) = (F-Kmat(:,:,t)*H)*Pmat(:,:,t)*(F-Kmat(:,:,t)*H)'+V1+Kmat(:,:,t)*V2*Kmat(:,:,t)'; % updating the state error covariance matrix (DRE) end % for yh(:,N) = H * xh(:,N); % a priori output estimation %% Plot the estimation results figure('Name','Observations'); hy1 = subplot(2,1,1); plot(timestep,y(1,:),'-ob', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'b' ); xlabel('time [s]'); ylabel('pos. along x-axis [m]'); hold on; plot(timestep,yh(1,:),'-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); % -------------- hy2 = subplot(2,1,2); plot(timestep,y(2,:),'-vb','LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'b'); xlabel('time [s]'); ylabel('pos. along y-axis [m]'); hold on; plot(timestep,yh(2,:),'-hr',... 'LineWidth',1.5, 'MarkerSize',8, 'MarkerFaceColor', 'r'); grid on leg2 = legend('Real observation $y_2$',... '1-step-ahead pred. observation $\hat{y}_2$',... 'location', 'best'); set(leg2,'Interpreter','latex'); set(leg2,'FontSize',18); set(gca, 'FontSize',14); linkaxes([hy1 hy2], 'x'); % =================================== figure('Name','State vs Predictions'); % ------------------- h11 = subplot(2,2,1); plot(timestep, X(1,:),'-ob', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'b' ); xlabel('time [s]'); ylabel('x_1 -> pos. along x-axis [m]'); hold on; plot(timestep, xh(1,:),'-vr', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'r' ); grid on xleg1 = legend('Real $x_1$',... '1 step ahead pred. $\hat{x}_1$',... 'location', 'best'); set(xleg1,'Interpreter','latex'); set(xleg1,'FontSize',18); set(gca, 'FontSize',14); % ------------------- h12 = subplot(2,2,2); plot(timestep, X(2,:),'-sb', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'b' ); xlabel('time [s]'); ylabel('x_2 -> speed along the x-axis[m/s]'); hold on; plot(timestep, xh(2,:),'-dr', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'r' ); grid on xleg2 = legend('Real $x_2$',... '1 step ahread pred. $\hat{x}_2$',... 'location', 'best'); set(xleg2,'Interpreter','latex'); set(xleg2,'FontSize',18); set(gca, 'FontSize',14); % ------------------- h21 = subplot(2,2,3); plot(timestep, X(3,:),'-hb', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'b' ); xlabel('time [s]'); ylabel('x_3 -> position along the y-axis[m/s]'); hold on; plot(timestep, xh(3,:),'-dr', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'r' ); grid on xleg3 = legend('Real $x_3$',... '1 step ahread pred. $\hat{x}_3$',... 'location', 'best'); set(xleg3,'Interpreter','latex'); set(xleg3,'FontSize',18); set(gca, 'FontSize',14); % ------------------- h22 = subplot(2,2,4); plot(timestep, X(4,:),'-hb', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'b' ); xlabel('time [s]'); ylabel('x_4 -> position along the y-axis[m/s]'); hold on; plot(timestep, xh(4,:),'-dr', 'LineWidth',1.5,... 'MarkerSize',8, 'MarkerFaceColor', 'r' ); grid on xleg4 = legend('Real $x_4$',... '1 step ahread pred. $\hat{x}_4$',... 'location', 'best'); set(xleg4,'Interpreter','latex'); set(xleg4,'FontSize',18); set(gca, 'FontSize',14); linkaxes([h11 h12 h21 h22], 'x');