Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
no_slip_bicycle_model_test.cpp
Go to the documentation of this file.
1#include "perception_sensor_lib/observation_model/ve/no_slip_bicycle_model.hpp"
2
3#include <gtest/gtest.h>
4
5#include <cmath>
6
7/* ----------------------- ODOMETRY MODEL -------------------------*/
8
13TEST(NoSlipBicycleModelTest, TestCgVelocityToWheels) {
14 common_lib::car_parameters::CarParameters car_parameters_;
15 car_parameters_.dist_cg_2_rear_axis = 1.0;
16 car_parameters_.wheelbase = 2.5;
17 car_parameters_.wheel_diameter = 0.6;
18 car_parameters_.gear_ratio = 4.0;
19 NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_);
20 Eigen::Vector3d cg_velocities(10.0, 2.0, 0.1); // Example velocities
21 Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities);
22
23 ASSERT_EQ(observations.size(), 6);
24
25 EXPECT_NEAR(observations(0), 325.584, 0.01); // front_wheels_rpm
26 EXPECT_NEAR(observations(1), 325.584, 0.01); // front_wheels_rpm
27 EXPECT_NEAR(observations(2), 324.004, 0.01); // rear_wheels_rpm
28 EXPECT_NEAR(observations(3), 324.004, 0.01); // rear_wheels_rpm
29 EXPECT_NEAR(observations(4), 0.02450, 0.001); // steering_angle
30 EXPECT_NEAR(observations(5), 1296.02, 0.01); // motor_rpm
31}
32
38TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsNegativeVx) {
39 common_lib::car_parameters::CarParameters car_parameters_;
40 car_parameters_.dist_cg_2_rear_axis = 1.0;
41 car_parameters_.wheelbase = 2.5;
42 car_parameters_.wheel_diameter = 0.6;
43 car_parameters_.gear_ratio = 4.0;
44 NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_);
45 Eigen::Vector3d cg_velocities(-10.0, 2.0, 0.1); // Example velocities
46 Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities);
47
48 // Check the size of the observations vector
49 ASSERT_EQ(observations.size(), 6);
50
51 EXPECT_NEAR(observations(0), -325.584, 0.01); // front_wheels_rpm
52 EXPECT_NEAR(observations(1), -325.584, 0.01); // front_wheels_rpm
53 EXPECT_NEAR(observations(2), -324.004, 0.01); // rear_wheels_rpm
54 EXPECT_NEAR(observations(3), -324.004, 0.01); // rear_wheels_rpm
55 EXPECT_NEAR(observations(4), 0.02450, 0.001); // steering_angle
56 EXPECT_NEAR(observations(5), -1296.02, 0.01); // motor_rpm
57}
58
64TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsZeroVx) {
65 common_lib::car_parameters::CarParameters car_parameters_;
66 car_parameters_.dist_cg_2_rear_axis = 1.0;
67 car_parameters_.wheelbase = 2.5;
68 car_parameters_.wheel_diameter = 0.6;
69 car_parameters_.gear_ratio = 4.0;
70 NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_);
71 Eigen::Vector3d cg_velocities(0.0, -2.0, -0.1); // Example velocities
72 Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities);
73
74 // Check the size of the observations vector
75 ASSERT_EQ(observations.size(), 6);
76
77 EXPECT_NEAR(observations(0), 68.4366, 0.01); // front_wheels_rpm
78 EXPECT_NEAR(observations(1), 68.4366, 0.01); // front_wheels_rpm
79 EXPECT_NEAR(observations(2), 60.47888, 0.01); // rear_wheels_rpm
80 EXPECT_NEAR(observations(3), 60.47888, 0.01); // rear_wheels_rpm
81 EXPECT_NEAR(observations(4), -0.1243, 0.001); // steering_angle
82 EXPECT_NEAR(observations(5), 241.91552, 0.01); // motor_rpm
83}
84
90TEST(NoSlipBicycleModelTest, TestCgVelocityToWheelsZeroVy) {
91 common_lib::car_parameters::CarParameters car_parameters_;
92 car_parameters_.dist_cg_2_rear_axis = 1.0;
93 car_parameters_.wheelbase = 2.5;
94 car_parameters_.wheel_diameter = 0.6;
95 car_parameters_.gear_ratio = 4.0;
96 NoSlipBicycleModel bicycle_model = NoSlipBicycleModel(car_parameters_);
97 Eigen::Vector3d cg_velocities(0.0, 0.0, 0.0); // Example velocities
98 Eigen::VectorXd observations = bicycle_model.expected_observations(cg_velocities);
99
100 // Check the size of the observations vector
101 ASSERT_EQ(observations.size(), 6);
102
103 EXPECT_NEAR(observations(0), 0, 0.01); // front_wheels_rpm
104 EXPECT_NEAR(observations(1), 0, 0.01); // front_wheels_rpm
105 EXPECT_NEAR(observations(2), 0, 0.01); // rear_wheels_rpm
106 EXPECT_NEAR(observations(3), 0, 0.01); // rear_wheels_rpm
107 EXPECT_NEAR(observations(4), 0, 0.001); // steering_angle
108 EXPECT_NEAR(observations(5), 0, 0.01); // motor_rpm
109}
110
116TEST(NoSlipBicycleModelTest, TestJacobianCgVelocityToWheels) {
117 // Initialize car parameters
118 common_lib::car_parameters::CarParameters car_parameters;
119 car_parameters.dist_cg_2_rear_axis = 1.0;
120 car_parameters.wheelbase = 2.5;
121 car_parameters.wheel_diameter = 0.6;
122 car_parameters.gear_ratio = 4.0;
123
124 // Initialize NoSlipBicycleModel with car parameters
125 NoSlipBicycleModel bicycle_model(car_parameters);
126
127 // Example velocities
128 Eigen::Vector3d cg_velocities(10.0, 2.0, 0.1);
129
130 // Calculate the Jacobian
131 Eigen::MatrixXd jacobian = bicycle_model.expected_observations_jacobian(cg_velocities);
132
133 // Check the size of the Jacobian matrix
134 ASSERT_EQ(jacobian.rows(), 6);
135 ASSERT_EQ(jacobian.cols(), 3);
136
137 EXPECT_NEAR(jacobian(0, 0), 31.1199, 0.01);
138 EXPECT_NEAR(jacobian(0, 1), 6.69077, 0.01);
139 EXPECT_NEAR(jacobian(0, 2), 10.0362, 0.01);
140 EXPECT_NEAR(jacobian(1, 0), 31.1199, 0.01);
141 EXPECT_NEAR(jacobian(1, 1), 6.69077, 0.01);
142 EXPECT_NEAR(jacobian(1, 2), 10.0362, 0.01);
143 EXPECT_NEAR(jacobian(2, 0), 31.2715, 0.01);
144 EXPECT_NEAR(jacobian(2, 1), 5.94159, 0.01);
145 EXPECT_NEAR(jacobian(2, 2), -5.9416, 0.01);
146 EXPECT_NEAR(jacobian(3, 0), 31.2715, 0.01);
147 EXPECT_NEAR(jacobian(3, 1), 5.94159, 0.01);
148 EXPECT_NEAR(jacobian(3, 2), -5.9416, 0.01);
149 EXPECT_NEAR(jacobian(4, 0), -0.002355, 0.01);
150 EXPECT_NEAR(jacobian(4, 1), -0.00047, 0.01);
151 EXPECT_NEAR(jacobian(4, 2), 0.24499, 0.01);
152 EXPECT_NEAR(jacobian(5, 0), 125.086, 0.01);
153 EXPECT_NEAR(jacobian(5, 1), 23.7664, 0.01);
154 EXPECT_NEAR(jacobian(5, 2), -23.7664, 0.01);
155}
156
163TEST(NoSlipBicycleModelTest, TestJacobianCgVelocityToWheelsZeroVy) {
164 // Initialize car parameters
165 common_lib::car_parameters::CarParameters car_parameters;
166 car_parameters.dist_cg_2_rear_axis = 1.0;
167 car_parameters.wheelbase = 2.5;
168 car_parameters.wheel_diameter = 0.6;
169 car_parameters.gear_ratio = 4.0;
170
171 // Initialize NoSlipBicycleModel with car parameters
172 NoSlipBicycleModel bicycle_model(car_parameters);
173
174 // Example velocities
175 Eigen::Vector3d cg_velocities(-10.0, 0, -0.1);
176
177 // Calculate the Jacobian
178 Eigen::MatrixXd jacobian = bicycle_model.expected_observations_jacobian(cg_velocities);
179
180 // Check the size of the Jacobian matrix
181 ASSERT_EQ(jacobian.rows(), 6);
182 ASSERT_EQ(jacobian.cols(), 3);
183
184 EXPECT_NEAR(jacobian(0, 0), 31.8274082, 0.01);
185 EXPECT_NEAR(jacobian(0, 1), 0.47741112, 0.01);
186 EXPECT_NEAR(jacobian(0, 2), 0.71611668, 0.01);
187 EXPECT_NEAR(jacobian(1, 0), 31.8274082, 0.01);
188 EXPECT_NEAR(jacobian(1, 1), 0.47741112, 0.01);
189 EXPECT_NEAR(jacobian(1, 2), 0.71611668, 0.01);
190 EXPECT_NEAR(jacobian(2, 0), 31.8293971, 0.01);
191 EXPECT_NEAR(jacobian(2, 1), -0.3182939, 0.01);
192 EXPECT_NEAR(jacobian(2, 2), 0.31829397, 0.01);
193 EXPECT_NEAR(jacobian(3, 0), 31.8293971, 0.01);
194 EXPECT_NEAR(jacobian(3, 1), -0.3182939, 0.01);
195 EXPECT_NEAR(jacobian(3, 2), 0.31829397, 0.01);
196 EXPECT_NEAR(jacobian(4, 0), -0.00149966, 0.01);
197 EXPECT_NEAR(jacobian(4, 1), 0.0, 0.01);
198 EXPECT_NEAR(jacobian(4, 2), 0.24984, 0.01);
199 EXPECT_NEAR(jacobian(5, 0), 127.3175884, 0.01);
200 EXPECT_NEAR(jacobian(5, 1), -1.27316, 0.01);
201 EXPECT_NEAR(jacobian(5, 2), 1.27316, 0.01);
202}
NoSlipBicycleModel::expected_observations
Eigen::VectorXd expected_observations(const Eigen::VectorXd &state) const override
Definition no_slip_bicycle_model.cpp:3
NoSlipBicycleModel::expected_observations_jacobian
Eigen::MatrixXd expected_observations_jacobian(const Eigen::VectorXd &state) const override
calculates the Jacobian of the expected_observations function with respect to the state.
Definition no_slip_bicycle_model.cpp:44
no_slip_bicycle_model.hpp
TEST
TEST(NoSlipBicycleModelTest, TestCgVelocityToWheels)
Test a regular case of the conversion from wheel velocities to cg velocities.
Definition no_slip_bicycle_model_test.cpp:13
common_lib::car_parameters::CarParameters
Definition car_parameters.hpp:15
common_lib::car_parameters::CarParameters::wheel_diameter
double wheel_diameter
Definition car_parameters.hpp:16
common_lib::car_parameters::CarParameters::gear_ratio
double gear_ratio
Definition car_parameters.hpp:21
common_lib::car_parameters::CarParameters::wheelbase
double wheelbase
Definition car_parameters.hpp:17
common_lib::car_parameters::CarParameters::dist_cg_2_rear_axis
double dist_cg_2_rear_axis
Definition car_parameters.hpp:20