Diff: EKF_RP.cpp

Revision:

1:6b803652d032

Parent:

0:a0e9705be9c4

Child:

4:e50e18eac72b

--- a/EKF_RP.cpp	Fri Oct 18 14:55:53 2019 +0000
+++ b/EKF_RP.cpp	Fri Oct 18 21:15:52 2019 +0000
@@ -58,10 +58,10 @@
 
 void EKF_RP::update_angles()
 {
-    s1 = sinf(x(0));
-    c1 = cosf(x(0));
-    s2 = sinf(x(1));
-    c2 = cosf(x(1));   
+    s1 = sin(x(0));
+    c1 = cos(x(0));
+    s2 = sin(x(1));
+    c2 = cos(x(1));   
 }
 
 void EKF_RP::update_F()
@@ -90,10 +90,10 @@
 {   
     Q << Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2)), (Ts*c1*s1*s2*var_fx(1))/c2,            0,            0,            0,            0,
                                (Ts*c1*s1*s2*var_fx(1))/c2,         Ts*c1*c1*var_fx(1),            0,            0,            0,            0,
-                                                        0,                          0, Ts*var_fx(3),            0,            0,            0,
-                                                        0,                          0,            0, Ts*var_fx(4),            0,            0,
-                                                        0,                          0,            0,            0, Ts*var_fx(5),            0,
-                                                        0,                          0,            0,            0,            0, Ts*var_fx(6);
+                                                        0,                          0, Ts*var_fx(2),            0,            0,            0,
+                                                        0,                          0,            0, Ts*var_fx(3),            0,            0,
+                                                        0,                          0,            0,            0, Ts*var_fx(4),            0,
+                                                        0,                          0,            0,            0,            0, Ts*var_fx(5);
 }
 
 void EKF_RP::update_Q()