Diff: solaESKF.cpp

Revision:

84:fb14acabefd3

Parent:

83:8f6ae61d47ac

Child:

85:552fe30e7f51

Child:

86:cc2c0025d820

--- a/solaESKF.cpp	Wed Jun 29 07:56:42 2022 +0000
+++ b/solaESKF.cpp	Wed Jul 13 05:08:01 2022 +0000
@@ -556,6 +556,40 @@
     errState = VectorXf::Zero(nState);
 }
 
+void solaESKF::fuseErr2Nominal(VectorXf errVal)
+{
+    //position
+    pihat(0) += errVal(0);
+    pihat(1) += errVal(1);
+    pihat(2) += errVal(2);
+
+    //velocity
+    vihat(0) += errVal(3);
+    vihat(1) += errVal(4);
+    vihat(2) += errVal(5);
+
+    //angle error
+    Vector4f qerr;
+    qerr << 1.0f, 0.5f*errVal(6), 0.5f*errVal(7), 0.5f*errVal(8);
+    qhat = quatmultiply(qhat, qerr);
+    qhat.normalize();
+
+    //acc bias
+    accBias(0) += errVal(9);
+    accBias(1) += errVal(10);
+    accBias(2) += errVal(11);
+
+    //gyro bias
+    gyroBias(0) += errVal(12);
+    gyroBias(1) += errVal(13);
+    gyroBias(2) += errVal(14);
+
+    //gravity bias
+    //gravity(0) += errVal(15);
+    //gravity(1) += errVal(16);
+    //gravity(2) += errVal(17);
+}
+
 Vector4f solaESKF::quatmultiply(Vector4f p, Vector4f q)
 {
     Vector4f qout;