Add accelerometer and magnetometer biases.

diff --git a/config/ekf.yaml b/config/ekf.yaml
index c6fb9e5..88409d8 100644 (file)
--- 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".
 
 
         # 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]
         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 (file)
--- 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);
 
        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);
        Pw.setIdentity();
        std::vector<double> PwDiag;
        declare_parameter("Pw",rclcpp::PARAMETER_DOUBLE_ARRAY);