Fixing measurement update systems for kalman filter

The equivalent octave code implementation taken from technical
paper is given below.

for j =1:6
	% compute Kalman gain
	K(:,j) = P(j ,:)./(P(j,j)+R(j,j));
	% update state vector
	x(:,i) = x(:,i) + K(:,j) * e(j);
	% update covariance matrix
	P = (eye(6) - (K(:,j) * H(j,:))) * P;
end

*** The actual fix for Nan issue is in seperate commit ***

Change-Id: Ided63f4df431416cb4ace4508417e2dffbd1888a

diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index c08325e..343b12a 100644 (file)
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -269,24 +269,28 @@ template <typename TYPE>
 inline void orientation_filter<TYPE>::measurement_update()
 {
        matrix<TYPE> gain(M6X6R, M6X6C);
-       TYPE iden = 0;
+       matrix<TYPE> iden(M6X6R, M6X6C);
+       iden.m_mat[0][0] = iden.m_mat[1][1] = iden.m_mat[2][2] = 1;
+       iden.m_mat[3][3] = iden.m_mat[4][4] = iden.m_mat[5][5] = 1;
 
        for (int j=0; j<M6X6C; ++j) {
-               for (int i=0; i<M6X6R; ++i)     {
+               for (int i=0; i<M6X6R; ++i) {
                        gain.m_mat[i][j] = m_pred_cov.m_mat[j][i] / (m_pred_cov.m_mat[j][j] + m_aid_cov.m_mat[j][j]);
-
                        m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
+               }
 
-                       if (i == j)
-                               iden = 1;
-                       else
-                               iden = 0;
+               matrix<TYPE> temp = iden;
 
-                       m_pred_cov.m_mat[j][i] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) *
-                                       m_pred_cov.m_mat[j][i];
+               for (int i=0; i<M6X6R; ++i)
+                       temp.m_mat[i][j] = iden.m_mat[i][j] - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i]);
 
-                       if (ABS(m_pred_cov.m_mat[j][i]) < NEGLIGIBLE_VAL)
-                               m_pred_cov.m_mat[j][i] = NEGLIGIBLE_VAL;
+               m_pred_cov = temp * m_pred_cov;
+       }
+
+       for (int j=0; j<M6X6C; ++j) {
+               for (int i=0; i<M6X6R; ++i) {
+                       if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
+                               m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
                }
        }