From 7a19e8938c99afe470556ca9e0ac5b22a64d39d3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 5 Jun 2020 14:11:41 +0200
Subject: [PATCH] Fix sigma value with the square root
---
test/gtest_odom_2d.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index be169a04a..b4c3e267b 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -172,7 +172,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
ASSERT_POSE2d_APPROX(F0->getStateVector(), Vector3d(0,0,0), 1e-6);
auto P0_vector = P0.vector("PO");
- ASSERT_MATRIX_APPROX(P0_solver, (P0_vector.array() * P0_vector.array()).matrix(), 1e-6);
+ ASSERT_MATRIX_APPROX(P0_solver, MatrixXd((P0_vector.array() * P0_vector.array()).matrix().asDiagonal()), 1e-6);
ASSERT_POSE2d_APPROX(F1->getStateVector(), Vector3d(2,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
ASSERT_POSE2d_APPROX(F2->getStateVector(), Vector3d(4,0,0), 1e-6);
@@ -329,7 +329,7 @@ TEST(Odom2d, KF_callback)
TimeStamp t0(0.0), t = t0;
double dt = .01;
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
- VectorComposite x0_cov(Vector3d(0.1,0.1,0.1), "PO", {2,1});
+ VectorComposite x0_cov(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
VectorXd data(Vector2d(1, M_PI/4) ); // advance 1m
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
int N = 8; // number of process() steps
--
GitLab