From afec077c31de4e78477b460417e5caaed9d0ac82 Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 12 Sep 2025 03:50:04 -0300
Subject: [PATCH] Add accelerometer and magnetometer biases.

---
 config/ekf.yaml | 2 +-
 src/ekf.cpp     | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

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);
-- 
2.12.0