Adjust UKF example to enable EKF comparison

matlab_simulation/01-examples_lecture/mr-5-unscentedkf/ukf.m

@@ -6,23 +6,23 @@
 dt = 0.1;
 
 % Initial State
-x0 = [20 3]';   %x position and y position
-v = [-2 0]';    %x speed and y speed respectively
+x0 = [20 -2 3]'; % horizontal distance, horizontal velocity, height
+v = [-2 0 0]';   % horizontal speed, horizontal acceleration, and vertical speed; respectively
 
 % Prior
-mu = [22 6]'; % mean (mu)
-S = 1*eye(2);% covariance (Sigma)
+mu = [22 -1.8 6.5]'; % mean (mu)
+S = 1*eye(3);% covariance (Sigma)
 mu_u = mu;
 S_u = S;
 
 % Discrete motion model
 % Separated the input out of the state matrix (A) to be more standard
 % With matrix A for state and matrix B for input
-Ad = [1 0; 0 1];    % State matrix for position
-Bd = [1 0; 0 1];    % Input matrix for speed
+Ad = eye(3);
+Bd = eye(3);
 
 % Position disturbance
-R = [0.001 0 ; 0 0.001];  %added extra disturbance in y motion
+R = [0.001 0 0; 0 0 0; 0 0 0.001];
 [RE, Re] = eig (R);
 
 % Measurement disturbance
@@ -67,16 +67,16 @@
     % Select a motion disturbance
     d = sqrt(Q)*randn(m,1);
     % Determine measurement
-    y(:,t) = sqrt(x(1,t)^2 + x(2,t)^2) + d;
-
+    y(:,t) = sqrt(x(1,t)^2 + x(3,t)^2) + d;
 
+
     %% Extended Kalman Filter Estimation (not working, wrong model for now)
-    % [mup,mu,S,K] = ekf(mu, S, y(:,t), @airplane_radar_motion_model, @airplane_radar_measurement_model, @airplane_radar_linearized_motion_model, @airplane_radar_linearized_measurement_model, Q, R, v);
+    [mu,S,mup,K] = ekf(mu, S, y(:,t), @airplane_radar_motion_model, @airplane_radar_measurement_model, @airplane_radar_linearized_motion_model, @airplane_radar_linearized_measurement_model, Q, R, v);
 
     % Store results
-    % mup_S(:,t) = mup;
-    % mu_S(:,t) = mu;
-    % K_S(:,t) = K;
+    mup_S(:,t) = mup;
+    mu_S(:,t) = mu;
+    K_S(:,t) = K;
 
     %% Unscented Kalman Filter Estimation
     % Prediction update
@@ -113,4 +113,5 @@
 end
 
 %% UKF and EFK error comparison
-%CompareEKF_UKF (T,x,mu_S,mu_Su);
+figure();
+CompareEKF_UKF (T,x,mu_S,mu_Su);

matlab_simulation/05-estimation/UKF/PlotData.m

@@ -2,26 +2,29 @@ function PlotData(t,x,mu_u,mu_S,mu_Su,mu,S,S_u)
 %Plots the UKF,EKF,True state and ground
     clf; hold on;
     % True state
-    plot(x(1,2:t),x(2,2:t), 'ro--')
+    plot(x(1,2:t),x(3,2:t), 'ro--')
     % EKF
-    plot(mu_S(1,2:t),mu_S(2,2:t), 'bx--')
+    plot(mu_S(1,2:t),mu_S(3,2:t), 'bx--')
     % UKF
-    plot(mu_Su(1,2:t),mu_Su(2,2:t), 'gx--')
+    plot(mu_Su(1,2:t),mu_Su(3,2:t), 'gx--')
     % EKF Ellipses
-    mu_pos = [mu(1) mu(2)];
-    S_pos = [S(1,1) S(1,2); S(2,1) S(2,2)];
+    mu_pos = [mu(1) mu(3)];
+    S_pos = [S(1,1) S(1,3); S(3,1) S(3,3)];
     error_ellipse(S_pos,mu_pos,0.75);
     error_ellipse(S_pos,mu_pos,0.95);
     % UKF Ellipses
-    mu_pos_u = [mu_u(1) mu_u(2)];
-    S_pos_u = [S_u(1,1) S_u(1,2); S_u(2,1) S_u(2,2)];
+    mu_pos_u = [mu_u(1) mu_u(3)];
+    S_pos_u = [S_u(1,1) S_u(1,3); S_u(3,1) S_u(3,3)];
     error_ellipse(S_pos_u,mu_pos_u,0.75);
     error_ellipse(S_pos_u,mu_pos_u,0.95);
     % Ground
     Ground(0,0)
     %Graph properties
     title('True state, EKF and UKF')
-    axis([-5 20 -1 10])
+    axis equal
+    axis([-1 20 -10 10])
+    xlabel('ground distance');
+    ylabel('height');
     legend('True state', 'EKF', 'UKF')
     F(t-1) = getframe;
 

matlab_simulation/05-estimation/UKF/SelectSigPoints.m

@@ -14,8 +14,8 @@
         x(:,n+i+1) = A*xp(:,n+i+1) + B*u*dt;
     elseif type == 2
         % Measurement model applied to sigma points
-        x(:,i+1) = sqrt(xp(1,i+1)^2 + xp(2,i+1)^2);
-        x(:,n+i+1) = sqrt(xp(1,n+i+1)^2 + xp(2,n+i+1)^2);
+        x(:,i+1) = sqrt(xp(1,i+1)^2 + xp(3,i+1)^2);
+        x(:,n+i+1) = sqrt(xp(1,n+i+1)^2 + xp(3,n+i+1)^2);
     end
 end
 

matlab_simulation/05-estimation/UKF/Update.m

@@ -9,7 +9,7 @@
     if type == 1
         model(:,1) = A*prediction(:,1) + B*u*dt; %need to change per example
     elseif type == 2
-        model(:,1) = sqrt(prediction(1,1)^2+prediction(2,1)^2); %need to change per example
+        model(:,1) = sqrt(prediction(1,1)^2+prediction(3,1)^2); %need to change per example
     end
 end