Fix sign error in dFY_dP part of transferFunctionJacobian_ (cra-ros-p…

…kg#592) Co-authored-by: Aleksander Bojda <a.bojda@agribot.eu>
clearpathrobotics · Oct 12, 2021 · 5060ec4 · 5060ec4
1 parent 704b223
commit 5060ec4
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/ekf.cpp b/src/ekf.cpp
@@ -323,7 +323,7 @@ namespace RobotLocalization
     zCoeff = -sp * cr;
     double dFz_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
                     (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
-    double dFY_dP = (sr * tp * cpi * pitchVel - cr * tp * cpi * yawVel) * delta;
+    double dFY_dP = (sr * tp * cpi * pitchVel + cr * tp * cpi * yawVel) * delta;
 
     // Much of the transfer function Jacobian is identical to the transfer function
     transferFunctionJacobian_ = transferFunction_;