Adding comparison figure

stateEstimation/KKF.m

@@ -1,12 +1,16 @@
 % Kinematic Kalman Filter for position and acceleration measurements
 
 meas = load("meas.mat");
+time = load("time.mat");
 
 x_pos = meas.xx(1,:);
 y_pos = meas.xx(2,:);
 
-sp = 1.0; % Sigma for position
+sp = 0.1; % Sigma for position
+p_x = 0;
+p_y = 0;
 m = length(x_pos); % Number of measurements
+time = time.t;
 
 % position measurement
 mpx = x_pos + sp .* randn(1,m);
@@ -21,7 +25,8 @@
 may = ay + sa .* randn(1, m);
 
 % Stack position and acceleration measurements
-meas = [mpx; mpy; max; may];
+% meas = [mpx; mpy; max; may];
+meas = [mpx; mpy];
 
 %% Setup matrices
 
@@ -35,17 +40,22 @@
      0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
 
 % Measurement function (Position and acceleration) 
+% H = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+%     0.0, 1.0, 0.0, 0.0, 0.0, 0.0;
+%     0.0, 0.0, 0.0, 0.0, 1.0, 0.0;
+%     0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
 H = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
-    0.0, 1.0, 0.0, 0.0, 0.0, 0.0;
-    0.0, 0.0, 0.0, 0.0, 1.0, 0.0;
-    0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
+    0.0, 1.0, 0.0, 0.0, 0.0, 0.0];
+
 
 ra = 10.0^2;   % Noise of Acceleration Measurement
-rp = 100.0^2;  % Noise of Position Measurement
-R = [rp, 0.0, 0.0, 0.0;
-    0.0, rp, 0.0, 0.0;
-    0.0, 0.0, ra, 0.0;
-    0.0, 0.0, 0.0, ra];
+rp = 0.1^2;  % Noise of Position Measurement
+% R = [rp, 0.0, 0.0, 0.0;
+%     0.0, rp, 0.0, 0.0;
+%     0.0, 0.0, ra, 0.0;
+%     0.0, 0.0, 0.0, ra];
+R = [rp, 0.0;
+     0.0, rp];
 
 sa = 0.001;
 G = [1/2.0*dt^2;
@@ -94,3 +104,42 @@
     % Cache data
     xEst = [xEst, x];
 end
+
+%% Visualization
+
+figure(1)
+subplot(221)
+plot(time, xEst(1,1:m-1),'k','linewidth',1.5); hold on
+plot(t(N_MHE+1:end),X_estimate(:,1),'r','linewidth',1.5); hold on
+plot(time, x_pos(1:m-1), 'g', 'linewidth', 1.5);hold on
+xlabel("Time[Sec]")
+ylabel("X[m]")
+legend("KF", "MHE", "GT")
+
+subplot(222)
+plot(time, xEst(2,1:m-1), 'k', 'linewidth',1.5); hold on
+plot(t(N_MHE+1:end),X_estimate(:,2),'r','linewidth',1.5); hold on
+plot(time, y_pos(1:m-1), 'g', 'linewidth', 1.5); hold on
+xlabel("Time[Sec]")
+ylabel("Y[m]")
+
+Ex_kf = abs(xEst(1,:) - x_pos);
+Ex_mhe = abs(X_estimate(:,1)' - x_pos(N_MHE+1:end-1));
+subplot(223)
+plot(time, Ex_kf(1:m-1), 'k', 'linewidth', 1.5); hold on;
+plot(time(N_MHE+1:end), Ex_mhe, 'r', 'linewidth', 1.5); hold on;
+
+xlabel("Time[Sec]")
+ylabel("Ex")
+
+Ey_kf = abs(xEst(2,:) - y_pos);
+Ey_mhe = abs(X_estimate(:,2)' - y_pos(N_MHE+1:end-1));
+subplot(224)
+plot(time, Ey_kf(1:m-1), 'k', 'linewidth', 1.5); hold on;
+plot(time(N_MHE+1:end), Ey_mhe, 'r', 'linewidth', 1.5); hold on;
+
+xlabel("Time[Sec]")
+ylabel("Ey")
+
+suptitle("MHE-KF Estimation Comparison")
+

stateEstimation/MHE.m

@@ -133,7 +133,7 @@
 toc
 
 ss_error = norm((x0-xs),2)
-Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam)
+% Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam)
 %-----------------------------------------
 %-----------------------------------------
 %-----------------------------------------
@@ -164,8 +164,8 @@
 r = [];
 alpha = [];
 for k = 1: length(xx(1,:))-1
-    r = [r; sqrt(xx(1,k)^2+xx(2,k)^2)  + sqrt(meas_cov(1,1))*randn(1)];
-    alpha = [alpha; atan(xx(2,k)/xx(1,k))      + sqrt(meas_cov(2,2))*randn(1)];
+    r = [r; sqrt(xx(1,k)^2+xx(2,k)^2) + sqrt(meas_cov(1,1))*randn(1)];
+    alpha = [alpha; atan(xx(2,k)/xx(1,k)) + sqrt(meas_cov(2,2))*randn(1)];
 end
 y_measurements = [ r , alpha ];
 
@@ -316,6 +316,7 @@
 end;
 %%
 save("meas.mat", "xx");
+save("time.mat", "t");
 %%
 figure(1)
 subplot(221)