Fix covariances according to datasheets.

diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp
index 85381ef..eee57ba 100644 (file)
--- a/test/ekf_test.cpp
+++ b/test/ekf_test.cpp
@@ -30,6 +30,8 @@
 #include <bno080.hpp>
 #include <ekf.hpp>
 
+#define sqr(x) ((x)*(x))
+
 enum chip
 {
        BNO055,
@@ -182,15 +184,58 @@ int main(int argc,char *argv[])
        P0.setIdentity();
        P0*=1e5;
 
+       const double T=0.02;    // 0.01 is too fast for an UART at 115200 bps
+       
        Eigen::MatrixXd Pw(7,7);
        Pw.setIdentity();
-       Pw*=1e-3*1e-3;
 
        Eigen::MatrixXd Pv(6,6);
        Pv.setIdentity();
-       Pv*=1e-3*1e-3;
 
-       const double T=0.02;    // 0.01 is too fast for an UART at 115200 bps
+       switch(chip)
+       {
+               case BNO055:
+               {
+                       double sigmag=0.014*M_PI/180.0/sqrt(T);
+                       double sigmaw=sqr(cos(sigmag/2.0))*sin(sigmag/2.0)+sqr(sin(sigmag/2.0))*cos(sigmag/2.0);
+                       Pw(0,0)=Pw(1,1)=Pw(2,2)=Pw(3,3)=sqr(sigmaw);
+                       Pw(4,4)=Pw(5,5)=Pw(6,6)=sqr(3.0*M_PI/180.0);
+
+                       Pv(0,0)=Pv(1,1)=Pv(2,2)=sqr(190e-6*9.81)/T;
+                       Pv(3,3)=Pv(4,4)=sqr(1e-6);
+                       Pv(5,5)=sqr(1.4e-6);
+                       break;
+               }
+               
+               case BNO080:
+               {
+                       double sigmag=3.1*M_PI/180.0;
+                       double sigmaw=sqr(cos(sigmag/2.0))*sin(sigmag/2.0)+sqr(sin(sigmag/2.0))*cos(sigmag/2.0);
+                       Pw(0,0)=Pw(1,1)=Pw(2,2)=Pw(3,3)=sqr(sigmaw);
+                       Pw(4,4)=Pw(5,5)=Pw(6,6)=sqr(3.1*M_PI/180.0);
+
+                       Pv(0,0)=Pv(1,1)=Pv(2,2)=sqr(0.3);
+                       Pv(3,3)=Pv(4,4)=Pv(5,5)=sqr(1.4e-6);
+                       break;
+               }
+               
+               case LSM9DS1:
+               {
+                       double sigmag=30.0*M_PI/180.0;
+                       double sigmaw=sqr(cos(sigmag/2.0))*sin(sigmag/2.0)+sqr(sin(sigmag/2.0))*cos(sigmag/2.0);
+                       Pw(0,0)=Pw(1,1)=Pw(2,2)=Pw(3,3)=sqr(sigmaw);
+                       Pw(4,4)=Pw(5,5)=Pw(6,6)=sqr(30.0*M_PI/180.0);
+
+                       Pv(0,0)=Pv(1,1)=Pv(2,2)=sqr(90e-3*9.81);
+                       Pv(3,3)=Pv(4,4)=Pv(5,5)=sqr(1.0*1e-4);
+                       break;
+               }
+
+               default:
+                       Pw*=sqr(1e-3);
+                       Pv*=sqr(1e-3);
+                       break;
+       }
 
        imufusion::Ekf ekf(q0,P0,Pw,Pv,T);