new fixed wing estimator: Fix the symmetry force step of the covariance prediction.

1 files changed, 11 insertions, 2 deletions

diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index 36b9f4491..e6f1fee87 100644
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@@ -876,7 +876,7 @@ void CovariancePrediction(float dt)
     nextP[20][19] = P[20][19];
     nextP[20][20] = P[20][20];
 
-    for (uint8_t i=0; i<= 20; i++)
+    for (unsigned i = 0; i < n_states; i++)
     {
         nextP[i][i] = nextP[i][i] + processNoise[i];
     }
@@ -942,7 +942,16 @@ void CovariancePrediction(float dt)
             P[i][i] = nextP[i][i];
         }
 
-        ForceSymmetry();
+        // force symmetry for observable states
+        for (unsigned i = 1; i < n_states; i++)
+        {
+            for (uint8_t j = 0; j < i; j++)
+            {
+                P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
+                P[j][i] = P[i][j];
+            }
+        }
+
     }
 
     ConstrainVariances();