From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Mon, 30 Sep 2019 02:19:38 +0000 (-0300)
Subject: Fix covariances according to datasheets.
X-Git-Tag: f1^0
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=a3aeeca2d6d51a7ecae675cf81a6cdf543c6aa93;p=imufusion.git

Fix covariances according to datasheets.
---

diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp
index 85381ef..eee57ba 100644
--- 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);