From 4ab1153c4a44a593d2d00a4e640488c809e255dc Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 22 Dec 2022 17:10:32 +1030 Subject: [PATCH] AP_Math: extend the control.cpp test suite --- libraries/AP_Math/tests/test_control.cpp | 451 ++++++++++++++++++++++- 1 file changed, 442 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Math/tests/test_control.cpp b/libraries/AP_Math/tests/test_control.cpp index ec60a4ffeb..217e23f382 100644 --- a/libraries/AP_Math/tests/test_control.cpp +++ b/libraries/AP_Math/tests/test_control.cpp @@ -9,27 +9,460 @@ TEST(Control, test_control) { postype_t pos_start = 17; float vel_start = 20; + float accel_start = 1.0; + const float dt = 0.01; + // test for update_pos_vel_accel includes update_vel_accel. + // test unlimited behaviour + // 1 float vel = vel_start; postype_t pos = pos_start; - const float dt = 0.01; - const float accel = 1.0; - - update_pos_vel_accel(pos, vel, accel, dt, 0, 0, 0); - EXPECT_FLOAT_EQ(vel, vel_start + accel*dt); - EXPECT_FLOAT_EQ(pos, pos_start + 0.5*(vel+vel_start)*dt); + float accel = accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 0.0, 0.0, 0.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + // 2 vel = vel_start; pos = pos_start; + accel = -accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 0.0, 0.0, 0.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // error has no impact when not limited + // 3 + vel = vel_start; + pos = pos_start; + accel = accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 0.0, 1.0, 1.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // 4 + vel = vel_start; + pos = pos_start; + accel = accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 0.0, -1.0, -1.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // test unlimited behaviour + // zero error should result in normal behaviour + // 5 + vel = vel_start; + pos = pos_start; + accel = accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 1.0, 0.0, 0.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // 6 + vel = vel_start; + pos = pos_start; + accel = -accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 1.0, 0.0, 0.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // 7 + vel = vel_start; + pos = pos_start; + accel = accel_start; + update_pos_vel_accel(pos, vel, accel, dt, -1.0, 0.0, 0.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // 8 + vel = vel_start; + pos = pos_start; + accel = -accel_start; + update_pos_vel_accel(pos, vel, accel, dt, -1.0, 0.0, 0.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // error sign opposite to limit sign should result in normal behaviour + // 9 + vel = vel_start; + pos = pos_start; + accel = accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 1.0, -1.0, -1.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // 10 + vel = vel_start; + pos = pos_start; + accel = -accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 1.0, -1.0, -1.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // 11 + vel = vel_start; + pos = pos_start; + accel = accel_start; + update_pos_vel_accel(pos, vel, accel, dt, -1.0, 1.0, 1.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // 12 + vel = vel_start; + pos = pos_start; + accel = -accel_start; + update_pos_vel_accel(pos, vel, accel, dt, -1.0, 1.0, 1.0); + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // error sign same as limit sign should result various limited behaviours + // 13 + vel = vel_start; + pos = pos_start; + accel = accel_start; update_pos_vel_accel(pos, vel, accel, dt, 1.0, 1.0, 1.0); + // vel is not increased EXPECT_FLOAT_EQ(vel, vel_start); + // pos is not increased EXPECT_FLOAT_EQ(pos, pos_start); + // 14 vel = vel_start; pos = pos_start; - update_pos_vel_accel(pos, vel, accel, dt, -1.0, 1.0, 1.0); - EXPECT_FLOAT_EQ(vel, vel_start + accel*dt); - EXPECT_FLOAT_EQ(pos, pos_start + 0.5*(vel+vel_start)*dt); + accel = -accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 1.0, 1.0, 1.0); + // vel is decreased + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + // pos is not increased + EXPECT_FLOAT_EQ(pos, pos_start); + + // 15 + vel = vel_start; + pos = pos_start; + accel = accel_start; + update_pos_vel_accel(pos, vel, accel, dt, -1.0, -1.0, -1.0); + // vel is increased + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + // pos is increased + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // 16 + vel = vel_start; + pos = pos_start; + accel = -accel_start; + update_pos_vel_accel(pos, vel, accel, dt, -1.0, -1.0, -1.0); + // velocity is limited but limit is not applied because velocity is reducing + EXPECT_FLOAT_EQ(vel, vel_start + accel * dt); + // pos is increased + EXPECT_FLOAT_EQ(pos, pos_start + vel_start * dt + 0.5 * accel * sq(dt)); + + // 17 + vel = -vel_start; + pos = pos_start; + accel = accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 1.0, 1.0, 1.0); + // velocity is limited but limit is not applied because velocity is reducing + EXPECT_FLOAT_EQ(vel, -vel_start + accel * dt); + // pos is decreased + EXPECT_FLOAT_EQ(pos, pos_start - vel_start * dt + 0.5 * accel * sq(dt)); + + // 18 + vel_start = 0.1 * accel_start * dt; + vel = vel_start; + pos = pos_start; + accel = -accel_start; + update_pos_vel_accel(pos, vel, accel, dt, -1.0, -1.0, -1.0); + // velocity is limited but limit is not applied because velocity is reducing + // final result is zero because velocity would change sign during dt + EXPECT_FLOAT_EQ(vel, 0.0); + // pos is not changed because is_negative(vel_start * dt + 0.5 * accel * sq(t)) + EXPECT_FLOAT_EQ(pos, pos_start); + + // 19 + vel = -vel_start; + pos = pos_start; + accel = accel_start; + update_pos_vel_accel(pos, vel, accel, dt, 1.0, 1.0, 1.0); + // velocity is limited but limit is not applied because velocity is reducing + // final result is zero because velocity would change sign during dt + EXPECT_FLOAT_EQ(vel, 0.0); + // pos is not changed because is_negative(vel_start * dt + 0.5 * accel * sq(t)) + EXPECT_FLOAT_EQ(pos, pos_start); + + + // test for update_pos_vel_accel includes update_vel_accel. + // test unlimited behaviour + + // 1 + pos_start = 17; + vel_start = 20; + accel_start = 1.0; + Vector2p posxy = Vector2p(pos_start, 0.0); + Vector2f velxy = Vector2f(vel_start, 0.0); + Vector2f accelxy = Vector2f(accel_start, 0.0); + Vector2f limit = Vector2f(0.0, 0.0); + Vector2f pos_error = Vector2f(0.0, 0.0); + Vector2f vel_error = Vector2f(0.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 2 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(-accel_start, 0.0); + limit = Vector2f(0.0, 0.0); + pos_error = Vector2f(0.0, 0.0); + vel_error = Vector2f(0.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // error has no impact when not limited + // 3 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(accel_start, 0.0); + limit = Vector2f(0.0, 0.0); + pos_error = Vector2f(1.0, 0.0); + vel_error = Vector2f(1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 4 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(accel_start, 0.0); + limit = Vector2f(0.0, 0.0); + pos_error = Vector2f(0.0, 0.0); + vel_error = Vector2f(0.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // test unlimited behaviour + // zero error should result in normal behaviour + // 5 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(accel_start, 0.0); + limit = Vector2f(1.0, 0.0); + pos_error = Vector2f(0.0, 0.0); + vel_error = Vector2f(0.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 6 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(-accel_start, 0.0); + limit = Vector2f(1.0, 0.0); + pos_error = Vector2f(0.0, 0.0); + vel_error = Vector2f(0.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 7 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(accel_start, 0.0); + limit = Vector2f(1.0, 0.0); + pos_error = Vector2f(0.0, 0.0); + vel_error = Vector2f(0.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 8 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(-accel_start, 0.0); + limit = Vector2f(1.0, 0.0); + pos_error = Vector2f(0.0, 0.0); + vel_error = Vector2f(0.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // error sign opposite to limit sign should result in normal behaviour + // 9 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(accel_start, 0.0); + limit = Vector2f(1.0, 0.0); + pos_error = Vector2f(-1.0, 0.0); + vel_error = Vector2f(-1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 10 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(-accel_start, 0.0); + limit = Vector2f(1.0, 0.0); + pos_error = Vector2f(-1.0, 0.0); + vel_error = Vector2f(-1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 11 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(accel_start, 0.0); + limit = Vector2f(-1.0, 0.0); + pos_error = Vector2f(1.0, 0.0); + vel_error = Vector2f(1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 12 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(-accel_start, 0.0); + limit = Vector2f(-1.0, 0.0); + pos_error = Vector2f(1.0, 0.0); + vel_error = Vector2f(1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // error sign same as limit sign should result various limited behaviours + // 13 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(accel_start, 0.0); + limit = Vector2f(1.0, 0.0); + pos_error = Vector2f(1.0, 0.0); + vel_error = Vector2f(1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + // vel is not increased + EXPECT_FLOAT_EQ(velxy.x, vel_start); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + // pos is not increased + EXPECT_FLOAT_EQ(posxy.x, pos_start); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 14 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(-accel_start, 0.0); + limit = Vector2f(1.0, 0.0); + pos_error = Vector2f(1.0, 0.0); + vel_error = Vector2f(1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + // vel is decreased + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + // pos is not increased + EXPECT_FLOAT_EQ(posxy.x, pos_start); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 15 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(accel_start, 0.0); + limit = Vector2f(-1.0, 0.0); + pos_error = Vector2f(-1.0, 0.0); + vel_error = Vector2f(-1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + // vel is increased + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + // pos is increased + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 16 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(-accel_start, 0.0); + limit = Vector2f(-1.0, 0.0); + pos_error = Vector2f(-1.0, 0.0); + vel_error = Vector2f(-1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + // velocity is limited but limit is not applied because velocity is reducing + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + // pos is increased + EXPECT_FLOAT_EQ(posxy.x, pos_start + vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 17 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(-vel_start, 0.0); + accelxy = Vector2f(accel_start, 0.0); + limit = Vector2f(1.0, 0.0); + pos_error = Vector2f(1.0, 0.0); + vel_error = Vector2f(1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + // velocity is limited but limit is not applied because velocity is reducing + EXPECT_FLOAT_EQ(velxy.x, -vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + // pos is decreased + EXPECT_FLOAT_EQ(posxy.x, pos_start - vel_start * dt + 0.5 * accelxy.x * sq(dt)); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 18 + vel_start = 0.1 * accel_start * dt; + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(vel_start, 0.0); + accelxy = Vector2f(-accel_start, 0.0); + limit = Vector2f(-1.0, 0.0); + pos_error = Vector2f(-1.0, 0.0); + vel_error = Vector2f(-1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + // velocity is limited but limit is not applied because velocity is reducing + // ideally this would be zero but code makes a simplification here + EXPECT_FLOAT_EQ(velxy.x, vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + // pos is not changed because is_negative(vel_start * dt + 0.5 * accel * sq(t)) + EXPECT_FLOAT_EQ(posxy.x, pos_start); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + + // 19 + posxy = Vector2p(pos_start, 0.0); + velxy = Vector2f(-vel_start, 0.0); + accelxy = Vector2f(accel_start, 0.0); + limit = Vector2f(1.0, 0.0); + pos_error = Vector2f(1.0, 0.0); + vel_error = Vector2f(1.0, 0.0); + update_pos_vel_accel_xy(posxy, velxy, accelxy, dt, limit, pos_error, vel_error); + // velocity is limited but limit is not applied because velocity is reducing + // ideally this would be zero but code makes a simplification here + EXPECT_FLOAT_EQ(velxy.x, -vel_start + accelxy.x * dt); + EXPECT_FLOAT_EQ(velxy.y, 0.0); + // pos is not changed because is_negative(vel_start * dt + 0.5 * accel * sq(t)) + EXPECT_FLOAT_EQ(posxy.x, pos_start); + EXPECT_FLOAT_EQ(velxy.y, 0.0); }