VelocitySmoothing - Refactor class to use polynomial evaluation instead of numerical integration

This solves many numerical issues when the trajectory is close to the
primary NE axes (small velocities). It is also more robust when dt is
large and has some jitter.

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -349,13 +349,13 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
 	Vector2f position_xy(_position);
 	Vector2f vel_traj_xy(_trajectory[0].getCurrentVelocity(), _trajectory[1].getCurrentVelocity());
 	Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy);
-	float position_error = drone_to_trajectory_xy.length();
+	//float position_error = drone_to_trajectory_xy.length();
 
-	float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f);
+	//float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f);
 
 	// Don't stretch time if the drone is ahead of the position setpoint
 	if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) {
-		time_stretch = 1.f;
+		//time_stretch = 1.f;
 	}
 
 	Vector3f jerk_sp_smooth;
@@ -364,23 +364,16 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
 	Vector3f pos_sp_smooth;
 
 	for (int i = 0; i < 3; ++i) {
-		_trajectory[i].integrate(_deltatime, time_stretch, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i));
+		// TODO: fix time stretch
+		//_trajectory[i].updateTraj(_time_stamp_current, time_stretch, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i));
+		_trajectory[i].updateTraj(_time, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i));
 		jerk_sp_smooth(i) = _trajectory[i].getCurrentJerk();
 	}
 
 	_updateTrajConstraints();
 
-	// If the acceleration and velocities are small and that we want to stop, reduce the amplitude of the jerk signal
-	// to help the optimizer to converge towards zero
-	if (Vector2f(_velocity_setpoint).length() < (0.01f * _param_mpc_xy_traj_p.get())
-	    && Vector2f(accel_sp_smooth).length() < 0.2f
-	    && Vector2f(vel_sp_smooth).length() < 0.1f) {
-		_trajectory[0].setMaxJerk(1.f);
-		_trajectory[1].setMaxJerk(1.f);
-	}
-
 	for (int i = 0; i < 3; ++i) {
-		_trajectory[i].updateDurations(_deltatime, _velocity_setpoint(i));
+		_trajectory[i].updateDurations(_time, _velocity_setpoint(i));
 	}
 
 	VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only

src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -155,7 +155,8 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
 
 	float pos_sp_smooth;
 
-	_smoothing.integrate(_acceleration_setpoint(2), _vel_sp_smooth, pos_sp_smooth);
+	// TODO: move before updateDurations
+	_smoothing.updateTraj(_time_stamp_current, _acceleration_setpoint(2), _vel_sp_smooth, pos_sp_smooth);
 	_velocity_setpoint(2) = _vel_sp_smooth; // Feedforward
 	_jerk_setpoint(2) = _smoothing.getCurrentJerk();
 

src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp

@@ -141,6 +141,13 @@ void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters()
 
 void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 {
+	Vector3f pos_sp_smooth;
+
+	for (int i = 0; i < 3; ++i) {
+		_smoothing[i].updateTraj(_time_stamp_current, _acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i));
+		_jerk_setpoint(i) = _smoothing[i].getCurrentJerk();
+	}
+
 	/* Get yaw setpont, un-smoothed position setpoints.*/
 	FlightTaskManualPosition::_updateSetpoints();
 
@@ -194,22 +201,9 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 		_position_lock_z_active = false;
 	}
 
-	// During position lock, lower jerk to help the optimizer
-	// to converge to 0 acceleration and velocity
-	if (_position_lock_xy_active) {
-		jerk[0] = 1.f;
-		jerk[1] = 1.f;
-
-	} else {
-		jerk[0] = _param_mpc_jerk_max.get();
-		jerk[1] = _param_mpc_jerk_max.get();
-	}
-
-	jerk[2] = _position_lock_z_active ? 1.f : _param_mpc_jerk_max.get();
-
 	for (int i = 0; i < 3; ++i) {
 		_smoothing[i].setMaxJerk(jerk[i]);
-		_smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i));
+		_smoothing[i].updateDurations(_time_stamp_current, _velocity_setpoint(i));
 	}
 
 	VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only
@@ -223,14 +217,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 		_smoothing[2].setCurrentPosition(_position(2));
 	}
 
-	Vector3f pos_sp_smooth;
-
-	for (int i = 0; i < 3; ++i) {
-		_smoothing[i].integrate(_acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i));
-		_velocity_setpoint(i) = _vel_sp_smooth(i); // Feedforward
-		_jerk_setpoint(i) = _smoothing[i].getCurrentJerk();
-	}
-
 	// Check for position lock transition
 	if (Vector2f(_vel_sp_smooth).length() < 0.1f &&
 	    Vector2f(_acceleration_setpoint).length() < .2f &&
@@ -276,6 +262,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
 		}
 	}
 
+	_velocity_setpoint = _vel_sp_smooth; // Feedforward
 	_position_setpoint(0) = _position_setpoint_xy_locked(0);
 	_position_setpoint(1) = _position_setpoint_xy_locked(1);
 	_position_setpoint(2) = _position_setpoint_z_locked;

src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -45,33 +45,31 @@ VelocitySmoothing::VelocitySmoothing(float initial_accel, float initial_vel, flo
 
 void VelocitySmoothing::reset(float accel, float vel, float pos)
 {
-	_jerk = 0.f;
-	_accel = accel;
-	_vel = vel;
-	_pos = pos;
+	_state.j = 0.f;
+	_state.a = accel;
+	_state.v = vel;
+	_state.x = pos;
 }
 
-float VelocitySmoothing::saturateT1ForAccel(float accel_prev, float max_jerk, float T1)
+float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max)
 {
 	/* Check maximum acceleration, saturate and recompute T1 if needed */
-	float accel_T1 = accel_prev + max_jerk * T1;
+	float accel_T1 = a0 + j_max * T1;
 	float T1_new = T1;
 
-	if (accel_T1 > _max_accel) {
-		T1_new = (_max_accel - accel_prev) / max_jerk;
+	if (accel_T1 > a_max) {
+		T1_new = (a_max - a0) / j_max;
 
-	} else if (accel_T1 < -_max_accel) {
-		T1_new = (-_max_accel - accel_prev) / max_jerk;
+	} else if (accel_T1 < -a_max) {
+		T1_new = (-a_max - a0) / j_max;
 	}
 
 	return T1_new;
 }
 
-float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_setpoint, float max_jerk)
+float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max)
 {
-	float b = 2.f * accel_prev / max_jerk;
-	float c = vel_prev / max_jerk + accel_prev * accel_prev / (2.f * max_jerk * max_jerk) - vel_setpoint / max_jerk;
-	float delta = b * b - 4.f * c;
+	float delta = 2.f * a0 * a0 + 4.f * j_max * v3;
 
 	if (delta < 0.f) {
 		// Solution is not real
@@ -79,11 +77,11 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s
 	}
 
 	float sqrt_delta = sqrtf(delta);
-	float T1_plus = (-b + sqrt_delta) * 0.5f;
-	float T1_minus = (-b - sqrt_delta) * 0.5f;
+	float T1_plus = (-a0 + 0.5f * sqrt_delta) / j_max;
+        float T1_minus = (-a0 - 0.5f * sqrt_delta) / j_max;
 
-	float T3_plus = accel_prev / max_jerk + T1_plus;
-	float T3_minus = accel_prev / max_jerk + T1_minus;
+	float T3_plus = a0 / j_max + T1_plus;
+	float T3_minus = a0 / j_max + T1_minus;
 
 	float T1 = 0.f;
 
@@ -94,21 +92,16 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s
 		T1 = T1_minus;
 	}
 
-	T1 = saturateT1ForAccel(accel_prev, max_jerk, T1);
-
-	if (T1 < _dt) {
-		T1 = 0.f;
-	}
+	T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
 
 	return math::max(T1, 0.f);
 }
 
-float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk)
+float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, float a_max)
 {
-	float a = -max_jerk;
-	float b = max_jerk * T123 - accel_prev;
-	float delta = T123 * T123 * max_jerk * max_jerk + 2.f * T123 * accel_prev * max_jerk - accel_prev * accel_prev
-		      + 4.f * max_jerk * (vel_prev - vel_setpoint);
+	float a = -j_max;
+	float b = j_max * T123 - a0;
+	float delta = T123 * T123 * j_max * j_max + 2.f * T123 *a0 * j_max - a0 * a0 - 4.f * j_max * v3;
 
 	if (delta < 0.f) {
 		// Solution is not real
@@ -120,8 +113,8 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev,
 	float T1_plus = math::max((-b + sqrt_delta) * denominator_inv, 0.f);
 	float T1_minus = math::max((-b - sqrt_delta) * denominator_inv, 0.f);
 
-	float T3_plus = accel_prev / max_jerk + T1_plus;
-	float T3_minus = accel_prev / max_jerk + T1_minus;
+	float T3_plus = a0 / j_max + T1_plus;
+	float T3_minus = a0 / j_max + T1_minus;
 
 	float T13_plus = T1_plus + T3_plus;
 	float T13_minus = T1_minus + T3_minus;
@@ -135,31 +128,20 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev,
 		T1 = T1_plus;
 	}
 
-	T1 = saturateT1ForAccel(accel_prev, max_jerk, T1);
-
-	if (T1 < _dt) {
-		T1 = 0.f;
-	}
+	T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
 
 	return T1;
 }
 
 
-float VelocitySmoothing::computeT2(float T1, float T3, float accel_prev, float vel_prev, float vel_setpoint,
-				   float max_jerk)
+float VelocitySmoothing::computeT2(float T1, float T3, float a0 , float v3, float j_max)
 {
-	float f = accel_prev * T1 + max_jerk * T1 * T1 * 0.5f + vel_prev + accel_prev * T3 + max_jerk * T1 * T3
-		  - max_jerk * T3 * T3 * 0.5f;
 	float T2 = 0.f;
 
-	float den = accel_prev + max_jerk * T1;
+	float den = a0 + j_max * T1;
 
 	if (math::abs_t(den) > FLT_EPSILON) {
-		T2 = (vel_setpoint - f) / den;
-	}
-
-	if (T2 < _dt) {
-		T2 = 0.f;
+		T2 = (-0.5f *T1 * T1 * j_max - T1 * T3*j_max - T1 * a0 + 0.5f * T3 * T3 * j_max - T3 * a0 + v3) / den;
 	}
 
 	return math::max(T2, 0.f);
@@ -171,56 +153,51 @@ float VelocitySmoothing::computeT2(float T123, float T1, float T3)
 	return math::max(T2, 0.f);
 }
 
-float VelocitySmoothing::computeT3(float T1, float accel_prev, float max_jerk)
+float VelocitySmoothing::computeT3(float T1, float a0, float j_max)
 {
-	float T3 = accel_prev / max_jerk + T1;
-
-	if (T1 < FLT_EPSILON && T3 < _dt && T3 > 0.f) {
-		T3 = _dt;
-		_max_jerk_T1 = accel_prev / T3;
-	}
+	float T3 = a0 / j_max + T1;
 
 	return math::max(T3, 0.f);
 }
 
-void VelocitySmoothing::integrateT(float dt, float jerk, float accel_prev, float vel_prev, float pos_prev,
-				   float &accel_out, float &vel_out, float &pos_out)
+void VelocitySmoothing::updateDurations(float t_now, float vel_setpoint)
 {
-	accel_out = jerk * dt + accel_prev;
+	_vel_sp = math::constrain(vel_setpoint, -_max_vel, _max_vel);
+	_t0 = t_now;
+	_state_init = _state;
 
-	vel_out = dt * 0.5f * (accel_out + accel_prev) + vel_prev;
+	/* Depending of the direction, start accelerating positively or negatively */
+	_direction = math::sign(_vel_sp - _state.v);
 
-	pos_out = dt / 3.f * (vel_out + accel_prev * dt * 0.5f + 2.f * vel_prev) + _pos;
-}
+	if (_direction != 0) {
+		updateDurations();
 
-void VelocitySmoothing::updateDurations(float dt, float vel_setpoint)
-{
-	_vel_sp = math::constrain(vel_setpoint, -_max_vel, _max_vel);
-	_dt = math::max(dt, FLT_EPSILON);
-	updateDurations();
+	} else {
+		_T1 = _T2 = _T3 = 0.f;
+	}
 }
 
 void VelocitySmoothing::updateDurations(float T123)
 {
 	float T1, T2, T3;
 
-	/* Depending of the direction, start accelerating positively or negatively */
-	_max_jerk_T1 = (_vel_sp - _vel > 0.f) ? _max_jerk : -_max_jerk;
+	float jerk_max_T1 = _direction * _max_jerk;
+	float delta_v = _vel_sp - _state.v;
 
 	// compute increasing acceleration time
 	if (T123 < 0.f) {
-		T1 = computeT1(_accel, _vel, _vel_sp, _max_jerk_T1);
+		T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel);
 
 	} else {
-		T1 = computeT1(T123, _accel, _vel, _vel_sp, _max_jerk_T1);
+		T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel);
 	}
 
 	// compute decreasing acceleration time
-	T3 = computeT3(T1, _accel, _max_jerk_T1);
+	T3 = computeT3(T1, _state.a, jerk_max_T1);
 
 	// compute constant acceleration time
 	if (T123 < 0.f) {
-		T2 = computeT2(T1, T3, _accel, _vel, _vel_sp, _max_jerk_T1);
+		T2 = computeT2(T1, T3, _state.a, delta_v, jerk_max_T1);
 
 	} else {
 		T2 = computeT2(T123, T1, T3);
@@ -231,52 +208,57 @@ void VelocitySmoothing::updateDurations(float T123)
 	_T3 = T3;
 }
 
-void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth,
-				  float &pos_setpoint_smooth)
+Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d)
 {
-	integrate(_dt, 1.f, accel_setpoint_smooth, vel_setpoint_smooth, pos_setpoint_smooth);
-}
+	Trajectory traj;
+	float jt = d * j;
+	float t2 = t * t;
+	float t3 = t2 * t;
 
-void VelocitySmoothing::integrate(float dt, float integration_scale_factor, float &accel_setpoint_smooth,
-				  float &vel_setpoint_smooth,
-				  float &pos_setpoint_smooth)
-{
-	/* Apply correct jerk (min, max or zero) */
-	if (_T1 > FLT_EPSILON) {
-		_jerk = _max_jerk_T1;
-
-		if (_T1 < dt && dt > _dt) {
-			// _T1 was supposed to be _dt, however, now, dt is bigger than _dt. We have to reduce the jerk to avoid an acceleration overshoot.
-			_jerk *= _dt / dt; // Keep the same area _dt * _jerk = dt * jerk_new
-		}
+	traj.j = jt;
+	traj.a = a0 + jt * t;
+	traj.v = v0 + a0 * t + 0.5f * jt * t2;
+	traj.x = x0 + v0 * t + 0.5f * a0 * t2 + 1.f / 6.f * jt * t3;
 
-	} else if (_T2 > FLT_EPSILON) {
-		_jerk = 0.f;
-
-	} else if (_T3 > FLT_EPSILON) {
-		_jerk = -_max_jerk_T1;
+	return traj;
+}
 
-		if (_T3 < dt && dt > _dt) {
-			// Same as for _T1 < dt above
-			_jerk *= _dt / dt;
-		}
+void VelocitySmoothing::updateTraj(float t_now, float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth)
+{
+	const float t = t_now - _t0;
+
+	if (t <= _T1) {
+		float t1 = t;
+		_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
+
+	} else if (t <= _T1 + _T2) {
+		float t1 = _T1;
+		float t2 = t - _T1;
+		_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
+		_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f);
+
+	} else if (t <= _T1 + _T2 + _T3) {
+		float t1 = _T1;
+		float t2 = _T2;
+		float t3 = t - _T1 - _T2;
+		_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
+		_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f);
+		_state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction);
 
 	} else {
-		_jerk = 0.f;
+		float t1 = _T1;
+		float t2 = _T2;
+		float t3 = _T3;
+		float t4 = t - _T1 - _T2 - _T3;
+		_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
+		_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f);
+		_state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction);
+		_state = evaluatePoly(0.f, 0.f, _state.v, _state.x, t4, 0.f);
 	}
 
-	/* Integrate the trajectory */
-	float accel_new, vel_new, pos_new;
-	integrateT(dt * integration_scale_factor, _jerk, _accel, _vel, _pos, accel_new, vel_new, pos_new);
-
-	_accel = accel_new;
-	_vel = vel_new;
-	_pos = pos_new;
-
-	/* set output variables */
-	accel_setpoint_smooth = _accel;
-	vel_setpoint_smooth = _vel;
-	pos_setpoint_smooth = _pos;
+	accel_setpoint_smooth = _state.a;
+	vel_setpoint_smooth = _state.v;
+	pos_setpoint_smooth = _state.x;
 }
 
 void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, int n_traj)

src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp

@@ -33,6 +33,13 @@
 
 #pragma once
 
+struct Trajectory {
+	float j; //< jerk
+	float a; //< acceleration
+	float v; //< velocity
+	float x; //< position
+};
+
 /**
  * @class VelocitySmoothing
  *
@@ -67,11 +74,11 @@ public:
 
 	/**
 	 * Compute T1, T2, T3 depending on the current state and velocity setpoint. This should be called on every cycle
-	 * and before integrate().
-	 * @param dt delta time between last updateDurations() call and now [s]
+	 * and before evaluateTraj().
 	 * @param vel_setpoint velocity setpoint input
+	 * @param t current time
 	 */
-	void updateDurations(float dt, float vel_setpoint);
+	void updateDurations(float t_now, float vel_setpoint);
 
 	/**
 	 * Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
@@ -81,27 +88,27 @@ public:
 	 * @param vel_setpoint_smooth returned smoothed velocity setpoint
 	 * @param pos_setpoint_smooth returned smoothed position setpoint
 	 */
-	void integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
-	void integrate(float dt, float integration_scale_factor, float &accel_setpoint_smooth, float &vel_setpoint_smooth,
-		       float &pos_setpoint_smooth);
+	void updateTraj(float t_now, float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
 
 	/* Get / Set constraints (constraints can be updated at any time) */
 	float getMaxJerk() const { return _max_jerk; }
 	void setMaxJerk(float max_jerk) { _max_jerk = max_jerk; }
 
+
+
 	float getMaxAccel() const { return _max_accel; }
 	void setMaxAccel(float max_accel) { _max_accel = max_accel; }
 
 	float getMaxVel() const { return _max_vel; }
 	void setMaxVel(float max_vel) { _max_vel = max_vel; }
 
-	float getCurrentJerk() const { return _jerk; }
-	void setCurrentAcceleration(const float accel) { _accel = accel; }
-	float getCurrentAcceleration() const { return _accel; }
-	void setCurrentVelocity(const float vel) { _vel = vel; }
-	float getCurrentVelocity() const { return _vel; }
-	void setCurrentPosition(const float pos) { _pos = pos; }
-	float getCurrentPosition() const { return _pos; }
+	float getCurrentJerk() const { return _state.j; }
+	void setCurrentAcceleration(const float accel) { _state.a = accel; }
+	float getCurrentAcceleration() const { return _state.a; }
+	void setCurrentVelocity(const float vel) { _state.v = vel; }
+	float getCurrentVelocity() const { return _state.v; }
+	void setCurrentPosition(const float pos) { _state.x = pos; }
+	float getCurrentPosition() const { return _state.x; }
 
 	/**
 	 * Synchronize several trajectories to have the same total time. This is required to generate
@@ -129,16 +136,16 @@ private:
 	/**
 	 * Compute increasing acceleration time
 	 */
-	inline float computeT1(float accel_prev, float vel_prev, float vel_setpoint, float max_jerk);
+	inline float computeT1(float a0, float v3, float j_max, float a_max);
 	/**
 	 * Compute increasing acceleration time using total time constraint
 	 */
-	inline float computeT1(float T123, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk);
-	inline float saturateT1ForAccel(float accel_prev, float max_jerk, float T1);
+	inline float computeT1(float T123, float a0, float v3, float j_max, float a_max);
+	inline float saturateT1ForAccel(float a0, float j_max, float T1, float a_max);
 	/**
 	 * Compute constant acceleration time
 	 */
-	inline float computeT2(float T1, float T3, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk);
+	inline float computeT2(float T1, float T3, float a0, float v3, float j_max);
 	/**
 	 * Compute constant acceleration time using total time constraint
 	 */
@@ -146,17 +153,22 @@ private:
 	/**
 	 * Compute decreasing acceleration time
 	 */
-	inline float computeT3(float T1, float accel_prev, float max_jerk);
+	inline float computeT3(float T1, float a0, float j_max);
 
 	/**
-	 * Integrate the jerk, acceleration and velocity to get the new setpoints and states.
+	 * Compute the jerk, acceleration, velocity and position
+	 * of a jerk-driven polynomial trajectory at a given time t
+	 * @param j jerk
+	 * @param a0 initial acceleration at t = 0
+	 * @param v0 initial velocity
+	 * @param x0 initial postion
+	 * @param t current time
+	 * @param d direction
 	 */
-	inline void integrateT(float dt, float jerk, float accel_prev, float vel_prev, float pos_prev,
-			       float &accel_out, float &vel_out, float &pos_out);
+	inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d);
 
-	/* Inputs */
+	/* Input */
 	float _vel_sp{0.0f};
-	float _dt = 1.f;
 
 	/* Constraints */
 	float _max_jerk = 22.f;
@@ -164,17 +176,16 @@ private:
 	float _max_vel = 6.f;
 
 	/* State (previous setpoints) */
-	float _jerk = 0.f;
-	float _accel = 0.f;
-	float _vel = 0.f;
-	float _pos = 0.f;
+	Trajectory _state{};
+	int _direction{0};
 
-	float _max_jerk_T1 = 0.f; ///< jerk during phase T1 (with correct sign)
+	/* Initial conditions */
+	Trajectory _state_init{};
 
 	/* Duration of each phase */
 	float _T1 = 0.f; ///< Increasing acceleration [s]
 	float _T2 = 0.f; ///< Constant acceleration [s]
 	float _T3 = 0.f; ///< Decreasing acceleration [s]
 
-	static constexpr float max_pos_err = 1.f; ///< maximum position error (if above, the position setpoint is locked)
+	float _t0 = 0.f; ///< Starting time
 };

src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp

@@ -48,7 +48,7 @@ class VelocitySmoothingTest : public ::testing::Test
 public:
 	void setConstraints(float j_max, float a_max, float v_max);
 	void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos);
-	void updateTrajectories(Vector3f velocity_setpoints, float dt);
+	void updateTrajectories(Vector3f velocity_setpoints, float t);
 
 	VelocitySmoothing _trajectories[3];
 };
@@ -71,19 +71,19 @@ void VelocitySmoothingTest::setInitialConditions(Vector3f a0, Vector3f v0, Vecto
 	}
 }
 
-void VelocitySmoothingTest::updateTrajectories(Vector3f velocity_setpoints, float dt)
+void VelocitySmoothingTest::updateTrajectories(Vector3f velocity_setpoints, float t)
 {
+	float dummy; // We don't care about the immediate result
+
 	for (int i = 0; i < 3; i++) {
-		_trajectories[i].updateDurations(dt, velocity_setpoints(i));
+		_trajectories[i].updateTraj(t, dummy, dummy, dummy);
 	}
 
-	VelocitySmoothing::timeSynchronization(_trajectories, 2);
-
-	float dummy; // We don't care about the immediate result
-
 	for (int i = 0; i < 3; i++) {
-		_trajectories[i].integrate(dummy, dummy, dummy);
+		_trajectories[i].updateDurations(t, velocity_setpoints(i));
 	}
+
+	VelocitySmoothing::timeSynchronization(_trajectories, 2);
 }
 
 TEST_F(VelocitySmoothingTest, testTimeSynchronization)
@@ -96,16 +96,16 @@ TEST_F(VelocitySmoothingTest, testTimeSynchronization)
 	setConstraints(j_max, a_max, v_max);
 
 	// AND: A set of initial conditions
-	Vector3f a0(0.f, 0.f, 0.f);
-	Vector3f v0(0.f, 0.f, 0.f);
+	Vector3f a0(0.22f, 0.f, 0.22f);
+	Vector3f v0(2.47f, -5.59e-6f, 2.47f);
 	Vector3f x0(0.f, 0.f, 0.f);
 
 	setInitialConditions(a0, v0, x0);
 
 	// WHEN: We generate trajectories (time synchronized in XY) with constant setpoints and dt
-	Vector3f velocity_setpoints(0.f, 1.f, 0.f);
-	float dt = 0.01f;
-	updateTrajectories(velocity_setpoints, dt);
+	Vector3f velocity_setpoints(-3.f, 1.f, 0.f);
+	updateTrajectories(velocity_setpoints, 0.f);
+
 
 	// THEN: The X and Y trajectories should have the same total time (= time sunchronized)
 	EXPECT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001);
@@ -114,27 +114,29 @@ TEST_F(VelocitySmoothingTest, testTimeSynchronization)
 TEST_F(VelocitySmoothingTest, testConstantSetpoint)
 {
 	// GIVEN: A set of initial conditions (same constraints as before)
-	Vector3f a0(0.22f, 0.f, 0.22f);
-	Vector3f v0(2.47f, -5.59e-6f, 2.47f);
+	Vector3f a0(0.f, 0.f, 0.f);
+	Vector3f v0(0.f, 0.f, 0.f);
 	Vector3f x0(0.f, 0.f, 0.f);
 
 	setInitialConditions(a0, v0, x0);
 
 	// WHEN: We generate trajectories with constant setpoints and dt
-	Vector3f velocity_setpoints(0.f, 1.f, 0.f);
-	float dt = 0.01f;
+	Vector3f velocity_setpoints(-3.f, 0.f, -1.f);
 
 	// Compute the number of steps required to reach desired value
 	// because of known numerical issues, the actual trajectory takes a
 	// bit more time than the predicted one, this is why we have to add 14 steps
 	// to the theoretical value.
 	// The updateTrajectories is fist called once to compute the total time
-	updateTrajectories(velocity_setpoints, dt);
+	float t = 0.f;
+	const float dt = 0.01;
+	updateTrajectories(velocity_setpoints, t);
 	float t123 = _trajectories[0].getTotalTime();
-	int nb_steps = ceil(t123 / dt) + 14;
+	int nb_steps = ceil(t123 / dt);
 
 	for (int i = 0; i < nb_steps; i++) {
-		updateTrajectories(velocity_setpoints, dt);
+		t += dt;
+		updateTrajectories(velocity_setpoints, t);
 	}
 
 	// THEN: All the trajectories should have reach their
@@ -156,11 +158,13 @@ TEST_F(VelocitySmoothingTest, testZeroSetpoint)
 
 	// AND: Null setpoints
 	Vector3f velocity_setpoints(0.f, 0.f, 0.f);
-	float dt = 0.01f;
+	float t = 0.f;
+	const float dt = 0.01f;
 
 	// WHEN: We run a few times the algorithm
 	for (int i = 0; i < 60; i++) {
-		updateTrajectories(velocity_setpoints, dt);
+		updateTrajectories(velocity_setpoints, t);
+		t += dt;
 	}
 
 	// THEN: All the trajectories should still be null

src/lib/FlightTasks/tasks/Utility/test_velocity_smoothing.cpp

@@ -43,8 +43,8 @@ int main(int argc, char *argv[])
 {
 	VelocitySmoothing trajectory[3];
 
-	float a0[3] = {0.22, 0.f, 0.22f};
-	float v0[3] = {2.47f, -5.59e-6f, 2.47f};
+	float a0[3] = {0.f, 0.f, 0.f};
+	float v0[3] = {0.f, 0.f, 0.f};
 	float x0[3] = {0.f, 0.f, 0.f};
 
 	float j_max = 55.2f;
@@ -59,18 +59,20 @@ int main(int argc, char *argv[])
 		trajectory[i].setCurrentVelocity(v0[i]);
 	}
 
-	float dt = 0.01f;
+	float t = 0.f;
+	const float dt = 0.01f;
 
-	float velocity_setpoint[3] = {0.f, 1.f, 0.f};
+	float velocity_setpoint[3] = {0.f, -1.f, 0.f};
 
 	for (int i = 0; i < 3; i++) {
-		trajectory[i].updateDurations(dt, velocity_setpoint[i]);
+		trajectory[i].updateDurations(velocity_setpoint[i], t);
 	}
 
-	VelocitySmoothing::timeSynchronization(trajectory, 2);
+	//VelocitySmoothing::timeSynchronization(trajectory, 2);
 
+	t += dt;
 	for (int i = 0; i < 3; i++) {
-		trajectory[i].integrate(a0[i], v0[i], x0[i]);
+		trajectory[i].updateTraj(t, a0[i], v0[i], x0[i]);
 	}
 
 	for (int i = 0; i < 3; i++) {