Diff: EKF.cpp

Revision:

20:1182bc29c195

Parent:

19:42ea6dd68185

--- a/EKF.cpp	Wed Oct 02 15:30:15 2019 +0000
+++ b/EKF.cpp	Wed Oct 09 13:45:36 2019 +0000
@@ -39,15 +39,15 @@
 void EKF::dgl(matrix *Z)
 {
     
-    x_km.a[0][0]= x.a[0][0]+Ts*(Z->a[0][0]-x.a[2][0]+tan(x.a[1][0])*sin(x.a[0][0])*(Z->a[1][0]-x.a[3][0]));
-    x_km.a[1][0]= x.a[1][0]+Ts*(cos(x.a[0][0])*(Z->a[1][0]-x.a[3][0]));
+    x_km.a[0][0]= x.a[0][0]+Ts*(Z->a[0][0]-x.a[2][0]+tan(x.a[1][0])*my_sin(x.a[0][0])*(Z->a[1][0]-x.a[3][0]));
+    x_km.a[1][0]= x.a[1][0]+Ts*(my_cos(x.a[0][0])*(Z->a[1][0]-x.a[3][0]));
     x_km.a[2][0]= x.a[2][0]+Ts*(-itau_g*x.a[2][0]);
     x_km.a[3][0]= x.a[3][0]+Ts*(-itau_g*x.a[3][0]);
-    x_km.a[4][0]= x.a[4][0]+Ts*(-g*sin(x.a[1][0])-kdm*x.a[4][0]);
-    x_km.a[5][0]= x.a[5][0]+Ts*(g*cos(x.a[1][0])*sin(x.a[0][0])-kdm*x.a[5][0]);
+    x_km.a[4][0]= x.a[4][0]+Ts*(-g*my_sin(x.a[1][0])-kdm*x.a[4][0]);
+    x_km.a[5][0]= x.a[5][0]+Ts*(g*my_cos(x.a[1][0])*my_sin(x.a[0][0])-kdm*x.a[5][0]);
     }
 // Just map gx ... to measurement matrix (to be consistent with Mahony etc.
-void EKF::update(float gx,float gy,float gz,float ax,float ax,float az){
+void EKF::update(float gx,float gy,float gz,float ax,float ay,float az){
     matrix measurement(6, 1, 0.0f);
     measurement.put_entry(0, 0, gx);
     measurement.put_entry(1, 0, gy);