From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 12 Sep 2025 06:50:04 +0000 (-0300)
Subject: Add accelerometer and magnetometer biases.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=HEAD;p=imufusion_ros.git

Add accelerometer and magnetometer biases.
---

diff --git a/config/ekf.yaml b/config/ekf.yaml
index c6fb9e5..88409d8 100644
--- a/config/ekf.yaml
+++ b/config/ekf.yaml
@@ -3,5 +3,5 @@ ekf:
 
         # Floating point numbers in YAML require the dot, even if using "e".
 
-        Pw: [7.47601e-07, 7.47601e-07, 7.47601e-07, 7.47601e-07, 0.00274156, 0.00274156, 0.00274156]
+        Pw: [7.47601e-07, 7.47601e-07, 7.47601e-07, 7.47601e-07, 0.00274156, 0.00274156, 0.00274156, 0.61591104, 0.61591104, 0.61591104, 1.6e-9, 1.6e-9, 1.6e-9]
         Pv: [0.000173706, 0.000173706, 0.000173706, 1.0e-12, 1.0e-12, 1.96e-12]
diff --git a/src/ekf.cpp b/src/ekf.cpp
index 285f3e3..188e38c 100644
--- a/src/ekf.cpp
+++ b/src/ekf.cpp
@@ -82,7 +82,7 @@ EkfNode::EkfNode(const char *name,const char *frameId):Node(name)
 	magSubscriber_=create_subscription<sensor_msgs::msg::MagneticField>("imu/mag",10,std::bind(&EkfNode::magCB,this,std::placeholders::_1));
 	imuPublisher_=create_publisher<sensor_msgs::msg::Imu>("imu/filtered",10);
 
-	Eigen::MatrixXd Pw(7,7);
+	Eigen::MatrixXd Pw(13,13);
 	Pw.setIdentity();
 	std::vector<double> PwDiag;
 	declare_parameter("Pw",rclcpp::PARAMETER_DOUBLE_ARRAY);