Skip to content
Snippets Groups Projects
Commit 7a19e893 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Fix sigma value with the square root

parent 17cd2a75
No related branches found
No related tags found
1 merge request!366Resolve "Complete state vector new data structure?"
Pipeline #5558 canceled
Stage: build
    Hide whitespace changes
    Inline Side-by-side
    Showing
    with 2 additions and 2 deletions
    test/gtest_odom_2d.cpp
    + 2
    − 2
    View file @ 7a19e893
    ...@@ -172,7 +172,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) ...@@ -172,7 +172,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
    ASSERT_POSE2d_APPROX(F0->getStateVector(), Vector3d(0,0,0), 1e-6); ASSERT_POSE2d_APPROX(F0->getStateVector(), Vector3d(0,0,0), 1e-6);
    auto P0_vector = P0.vector("PO"); 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_POSE2d_APPROX(F1->getStateVector(), Vector3d(2,0,0), 1e-6);
    ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6); ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
    ASSERT_POSE2d_APPROX(F2->getStateVector(), Vector3d(4,0,0), 1e-6); ASSERT_POSE2d_APPROX(F2->getStateVector(), Vector3d(4,0,0), 1e-6);
    ...@@ -329,7 +329,7 @@ TEST(Odom2d, KF_callback) ...@@ -329,7 +329,7 @@ TEST(Odom2d, KF_callback)
    TimeStamp t0(0.0), t = t0; TimeStamp t0(0.0), t = t0;
    double dt = .01; double dt = .01;
    VectorComposite x0(Vector3d(0,0,0), "PO", {2,1}); 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 VectorXd data(Vector2d(1, M_PI/4) ); // advance 1m
    Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01; Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
    int N = 8; // number of process() steps int N = 8; // number of process() steps
    ......
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment