Fixed issue with covariance augmentation. Modified plotting so that t…

…here is no auto-alignment for estimated landmark case. This makes the unobservable states more clear.
RossHartley · Sep 4, 2018 · 15f1ee7 · 15f1ee7
1 parent c20c0c8
commit 15f1ee7
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 53 deletions.
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1,2 @@
+slprj/
+*.asv
diff --git a/matlab_example/example_code/RIEKF.m b/matlab_example/example_code/RIEKF.m
@@ -549,7 +549,7 @@ function resetImpl(~)
             R_pred = obj.X(1:3,1:3);
 
             % Stack landmark measurements
-            Y = []; H = []; N = []; PI = []; new_landmarks = [];
+            Y = []; H = []; N = []; PI = []; new_landmarks = [];  
             for i = 1:size(landmarks,2)
                 % Search to see if measured landmark id is in the list of
                 % static landmarks
@@ -595,7 +595,7 @@ function resetImpl(~)
                     F = vertcat(F, [zeros(3,6), eye(3), zeros(3,3*(size(obj.X,2)-6))]); % Add row to increase dimension
                     F = blkdiag(F, eye(6)); % Add block I for parameters
                     G = zeros(size(F,1),3);
-                    G(end-8:end-6,:) = R_pred * obj.Ql;
+                    G(end-8:end-6,:) = R_pred;
                     obj.P = F*obj.P*F' + G*obj.Ql*G';
                 end
             end

diff --git a/matlab_example/example_code/plot_results.m b/matlab_example/example_code/plot_results.m
@@ -82,7 +82,8 @@
 end
 
 %% Start and end indices
-[~, startIndex] = max( diff(p(3,:)) ); 
+% [~, startIndex] = max( diff(p(3,:)) ); 
+startIndex = find(enabled.signals.values == 1, 1, 'first');
 startIndex = startIndex + 1;
 endIndex = length(p);
 
@@ -122,24 +123,25 @@
 ba_true = ba_true(:,startIndexTrue:endIndexTrue);
 
 %% Align starting absolute position and yaw
-if ~enable_static_landmarks
-    HW1B = [R(:,:,1), p(:,1); zeros(1,3), 1];
-    HW2B = [R_true(:,:,1), p_true(:,1); zeros(1,3), 1];
-    HW2W1 = HW2B/HW1B;
-    for i = 1:length(t)
-        % World Frame
-        R(:,:,i) = HW2W1(1:3,1:3) * R(:,:,i);
-        q(:,i) = Rotation_to_Euler(R(:,:,i));
-        v(:,i) = HW2W1(1:3,1:3) * v(:,i);
-        p(:,i) = HW2W1(1:3,end) + HW2W1(1:3,1:3) * p(:,i);
-
-        % Landmarks
-        for j = 1:length(lm)
-            lm{j}(2:4,i) = HW2W1(1:3,end) + HW2W1(1:3,1:3) * lm{j}(2:4,i);
-        end
-
-    end
-end
+% if ~enable_static_landmarks
+%     HW1B = [R(:,:,1), p(:,1); zeros(1,3), 1];
+%     HW2B = [R_true(:,:,1), p_true(:,1); zeros(1,3), 1];
+%     HW2W1 = HW2B/HW1B;
+%     for i = 1:length(t)
+%         % World Frame
+%         R(:,:,i) = HW2W1(1:3,1:3) * R(:,:,i);
+%         q(:,i) = Rotation_to_Euler(R(:,:,i));
+%         v(:,i) = HW2W1(1:3,1:3) * v(:,i);
+%         p(:,i) = HW2W1(1:3,end) + HW2W1(1:3,1:3) * p(:,i);
+%         
+%         % Landmarks
+%         for j = 1:length(lm)
+%             lm{j}(2:4,i) = HW2W1(1:3,end) + HW2W1(1:3,1:3) * lm{j}(2:4,i);
+%         end
+%         
+%     end
+% end
+
 %% Initialize Plots
 fignum = 1;
 EstimateLine = 'r-';
@@ -234,38 +236,38 @@
 xlabel('time (sec)')
 
 %% Landmark Plot
-for i = 1:length(lm)
-    if all(all(isnan(lm{i})))
-        continue;
-    end
-    figure(fignum)
-    fignum = fignum + 1;
-
-    subplot(3,1,1)
-    grid on; hold on;
-    plot(t, lm{i}(2,:), EstimateLine, 'LineWidth', LineWidth)
-    plot(t, lm_true{i}(2,:), TrueLine, 'LineWidth', LineWidth)
-    legend('Estimate', 'True')
-    title(['Landmark ', num2str(lm_true{i}(1)),' (World) - x'])
-    ylabel('m')
-
-    subplot(3,1,2)
-    grid on; hold on;
-    plot(t, lm{i}(3,:), EstimateLine, 'LineWidth', LineWidth)
-    plot(t, lm_true{i}(3,:), TrueLine, 'LineWidth', LineWidth)
-    legend('Estimate', 'True')
-    title(['Landmark ', num2str(lm_true{i}(1)),' (World) - y'])
-    ylabel('m')
-
-    subplot(3,1,3)
-    grid on; hold on;
-    plot(t, lm{i}(4,:), EstimateLine, 'LineWidth', LineWidth)
-    plot(t, lm_true{i}(4,:), TrueLine, 'LineWidth', LineWidth)
-    legend('Estimate', 'True')
-    title(['Landmark ', num2str(lm_true{i}(1)),' (World) - z'])
-    ylabel('m')
-    xlabel('time (sec)')
-end
+% for i = 1:length(lm)
+%     if all(all(isnan(lm{i})))
+%         continue;
+%     end
+%     figure(fignum)
+%     fignum = fignum + 1;
+%     
+%     subplot(3,1,1)
+%     grid on; hold on;
+%     plot(t, lm{i}(2,:), EstimateLine, 'LineWidth', LineWidth)
+%     plot(t, lm_true{i}(2,:), TrueLine, 'LineWidth', LineWidth)
+%     legend('Estimate', 'True')
+%     title(['Landmark ', num2str(lm_true{i}(1)),' (World) - x'])
+%     ylabel('m')
+%     
+%     subplot(3,1,2)
+%     grid on; hold on;
+%     plot(t, lm{i}(3,:), EstimateLine, 'LineWidth', LineWidth)
+%     plot(t, lm_true{i}(3,:), TrueLine, 'LineWidth', LineWidth)
+%     legend('Estimate', 'True')
+%     title(['Landmark ', num2str(lm_true{i}(1)),' (World) - y'])
+%     ylabel('m')
+%     
+%     subplot(3,1,3)
+%     grid on; hold on;
+%     plot(t, lm{i}(4,:), EstimateLine, 'LineWidth', LineWidth)
+%     plot(t, lm_true{i}(4,:), TrueLine, 'LineWidth', LineWidth)
+%     legend('Estimate', 'True')
+%     title(['Landmark ', num2str(lm_true{i}(1)),' (World) - z'])
+%     ylabel('m')
+%     xlabel('time (sec)')
+% end