Fixing Euler projection

cra-ros-pkg · Oct 24, 2022 · 1abfa34 · 1abfa34
1 parent 710d68e
commit 1abfa34
Showing 1 changed file with 6 additions and 6 deletions.
diff --git a/src/ukf.cpp b/src/ukf.cpp
@@ -320,12 +320,12 @@ namespace RobotLocalization
 
     generateSigmaPoints();
 
-    double roll_sum_x {};
-    double roll_sum_y {};
-    double pitch_sum_x {};
-    double pitch_sum_y {};
-    double yaw_sum_x {};
-    double yaw_sum_y {};
+    double roll_sum_x = ::cos(state_(StateMemberRoll));
+    double roll_sum_y = ::sin(state_(StateMemberRoll));
+    double pitch_sum_x = ::cos(state_(StateMemberPitch));
+    double pitch_sum_y = ::sin(state_(StateMemberPitch));
+    double yaw_sum_x = ::cos(state_(StateMemberYaw));
+    double yaw_sum_y = ::sin(state_(StateMemberYaw));
 
     // Sum the weighted sigma points to generate a new state prediction
     state_.setZero();