Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
observation_model_test.cpp
Go to the documentation of this file.
1#include <eigen3/Eigen/Dense>
2#include <random>
3
4#include "gtest/gtest.h"
5#include "kalman_filter/observation_models.hpp"
6
12TEST(BASE_OBSERVATION_MODEL, ALIGNED_RANDOM_TEST) {
13 Eigen::Matrix2f q_matrix = Eigen::Matrix2f::Zero();
14 ObservationModel observation_model(q_matrix);
15 ObservationData observation_data;
16 std::random_device rd; // Will be used to obtain a seed for the random number engine
17 std::mt19937 gen(rd());
18 std::uniform_int_distribution dis(-500, 500);
19 for (unsigned int i = 0; i < 1000; i++) {
20 Eigen::VectorXf expected_state(5);
21 float vehicle_x = static_cast<float>(dis(gen));
22 float vehicle_y = static_cast<float>(dis(gen));
23 float landmark_x = static_cast<float>(dis(gen));
24 float landmark_y = static_cast<float>(dis(gen));
25 expected_state << vehicle_x, vehicle_y, 0, landmark_x, landmark_y;
26
27 observation_data.position.x = landmark_x - vehicle_x;
28 observation_data.position.y = landmark_y - vehicle_y;
29 Eigen::Vector2f observed_landmark_position =
30 observation_model.inverse_observation_model(expected_state, observation_data);
31 Eigen::Vector2f predicted_observation = observation_model.observation_model(expected_state, 3);
32 EXPECT_EQ(observed_landmark_position(0), landmark_x);
33 EXPECT_EQ(observed_landmark_position(1), landmark_y);
34 EXPECT_EQ(predicted_observation(0), observation_data.position.x);
35 EXPECT_EQ(predicted_observation(1), observation_data.position.y);
36 }
37}
38
43TEST(BASE_OBSERVATION_MODEL, INDIVIDUAL_CASE_TEST1) {
44 Eigen::Matrix2f q_matrix = Eigen::Matrix2f::Zero();
45 ObservationModel observation_model(q_matrix);
46 ObservationData observation_data;
47 Eigen::VectorXf expected_state(5);
48 float vehicle_x = 5;
49 float vehicle_y = -2;
50 float orientation = static_cast<float>(-M_PI / 4);
51 float landmark_x = 5;
52 float landmark_y = static_cast<float>(-2 - sqrt(2));
53 observation_data.position.x = 1;
54 observation_data.position.y = -1;
55 expected_state << vehicle_x, vehicle_y, orientation, landmark_x, landmark_y;
56 Eigen::Vector2f observed_landmark_position =
57 observation_model.inverse_observation_model(expected_state, observation_data);
58 Eigen::Vector2f predicted_observation = observation_model.observation_model(expected_state, 3);
59 EXPECT_NEAR(observed_landmark_position(0), landmark_x, 0.00001);
60 EXPECT_NEAR(observed_landmark_position(1), landmark_y, 0.00001);
61 EXPECT_NEAR(predicted_observation(0), observation_data.position.x, 0.00001);
62 EXPECT_NEAR(predicted_observation(1), observation_data.position.y, 0.00001);
63}
64
69TEST(BASE_OBSERVATION_MODEL, INDIVIDUAL_CASE_TEST2) {
70 Eigen::Matrix2f q_matrix = Eigen::Matrix2f::Zero();
71 ObservationModel observation_model(q_matrix);
72 ObservationData observation_data;
73 Eigen::VectorXf expected_state(5);
74 float vehicle_x = 5;
75 float vehicle_y = -2;
76 float orientation = static_cast<float>(2 * M_PI);
77 float landmark_x = 6;
78 float landmark_y = -3;
79 observation_data.position.x = 1;
80 observation_data.position.y = -1;
81 expected_state << vehicle_x, vehicle_y, orientation, landmark_x, landmark_y;
82 Eigen::Vector2f observed_landmark_position =
83 observation_model.inverse_observation_model(expected_state, observation_data);
84 Eigen::Vector2f predicted_observation = observation_model.observation_model(expected_state, 3);
85 EXPECT_NEAR(observed_landmark_position(0), landmark_x, 0.00001);
86 EXPECT_NEAR(observed_landmark_position(1), landmark_y, 0.00001);
87 EXPECT_NEAR(predicted_observation(0), observation_data.position.x, 0.00001);
88 EXPECT_NEAR(predicted_observation(1), observation_data.position.y, 0.00001);
89}
90
95TEST(BASE_OBSERVATION_MODEL, INDIVIDUAL_CASE_TEST3) {
96 Eigen::Matrix2f q_matrix = Eigen::Matrix2f::Zero();
97 ObservationModel observation_model(q_matrix);
98 ObservationData observation_data;
99 Eigen::VectorXf expected_state(5);
100 float vehicle_x = 5;
101 float vehicle_y = -2;
102 float orientation = static_cast<float>(M_PI / 2);
103 float landmark_x = 6;
104 float landmark_y = -1;
105 observation_data.position.x = 1;
106 observation_data.position.y = -1;
107 expected_state << vehicle_x, vehicle_y, orientation, landmark_x, landmark_y;
108 Eigen::Vector2f observed_landmark_position =
109 observation_model.inverse_observation_model(expected_state, observation_data);
110 Eigen::Vector2f predicted_observation = observation_model.observation_model(expected_state, 3);
111 EXPECT_NEAR(observed_landmark_position(0), landmark_x, 0.00001);
112 EXPECT_NEAR(observed_landmark_position(1), landmark_y, 0.00001);
113 EXPECT_NEAR(predicted_observation(0), observation_data.position.x, 0.00001);
114 EXPECT_NEAR(predicted_observation(1), observation_data.position.y, 0.00001);
115}
ObservationModel
Observation Model class compiled of functions for observation model.
Definition observation_models.hpp:29
ObservationModel::observation_model
Eigen::Vector2f observation_model(const Eigen::VectorXf &expected_state, const unsigned int landmark_index) const
Calculate expected observation from the state vector.
Definition observation_models.cpp:56
ObservationModel::inverse_observation_model
Eigen::Vector2f inverse_observation_model(const Eigen::VectorXf &expected_state, const ObservationData &observation_data) const
Calculate landmark position from observation.
Definition observation_models.cpp:8
TEST
TEST(BASE_OBSERVATION_MODEL, ALIGNED_RANDOM_TEST)
Test the observation model in a scenario where the vehicle is always aligned with the x axis.
Definition observation_model_test.cpp:12
observation_models.hpp
ObservationData
Struct containing observation data.
Definition observation_models.hpp:14
ObservationData::position
common_lib::structures::Position position
Definition observation_models.hpp:15
common_lib::structures::Position::x
double x
Definition position.hpp:8
common_lib::structures::Position::y
double y
Definition position.hpp:9