From 5693a9d3b6e923b2a2c05594f10cb5366ee6c495 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 23 Mar 2014 18:28:13 +0100
Subject: [PATCH] new fixed wing estimator: Fix the symmetry force step of the
 covariance prediction.

---
 src/modules/fw_att_pos_estimator/estimator.cpp | 13 +++++++++++--
 1 file 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 36b9f4491b..e6f1fee874 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();