A key component of the UKF implementation, as well as the yet-to-be-developed NMPC system, is the dynamics model. Currently we’re using a simple centripetal model in the UKF:
AccelerationVector CentripetalModel::evaluate(
const State &in, const ControlVector &control) const {
AccelerationVector output;
/* First convert velocity to body frame. */
Eigen::Matrix velocity_body;
velocity_body = Quaternionr(in.attitude()) * in.velocity();
/* Calculate centripetal acceleration. */
output.segment<3>(0) = in.angular_velocity().cross(velocity_body);
/* Clear angular acceleration. */
output.segment<3>(3) << 0, 0, 0;
return output;
}
In our UKF implementation, the dynamics model is called with the current
UKF state, as well as the system control vector. The control vector is
model-defined, but in our case will be the control deflection in radians for
elevons, and the current motor RPM for the throttle. The DynamicsModel::evaluate(const State&, const ControlVector&)
method returns an AccelerationVector, which is a 6-element vector
containing linear and angular acceleration X, Y and Z components.
Based on the attitude error statistics returned by the horizon
detection code the simple CentripetalModel above yields a
reduction in error of about 2°, despite the significant lag in GPS velocity
relative to the accelerometer readings; it therefore seems likely that a
dynamics model that takes thrust, control surface position and aerodynamic
forces into consideration could result in further significant accuracy
improvements.
The first step in developing the model was reviewing existing implementations, most notably JSBSim as it’s commonly used as a reference FDM in papers on the topic. The JSBSim model is quite clear, but highly structured and considerably more complex than required for our application, so I also reviewed simpler from-scratch implementations (e.g. the FlightSim example from “Physics for Game Developers”) and of course the underlying physics.
The second, considerably more challenging step was determining how detailed the model should be—obviously a finite-element approach was out of the question, but how many simplifying assumptions should be made? For example, do we assume evenly-distributed mass? What about the effect of α and β on control surface forces? Do we consider lift, drag or moment due to β at all?
After some research it appeared that most aerodynamic coefficients depend non-linearly on α, and then approximately linearly on the relevant other parameter (β, yaw rate, pitch, rate, roll rate, and control surface deflection—assuming those angles stay “small”). Thus, we opted to use a quartic model to relate the coefficient values with α, and then multiply by the appropriate other parameter.
The coefficient model we decided to use was:
The resulting code pretty much wrote itself, and at an estimated 1500 DSP cycles per update, requires considerably less power than our budget of 2500 cycles (the UKF evaluates the model in double precision about 72,000 times per second, while the NMPC system will evaluate it in single precision something like 250,000 times per second).
AccelerationVector FixedWingFlightDynamicsModel::evaluate(
const State &in, const ControlVector &control) const {
/* Cache state data for convenience */
Quaternionr attitude = Quaternionr(in.attitude());
real_t yaw_rate = in.angular_velocity()[2],
pitch_rate = in.angular_velocity()[1],
roll_ratre = in.angular_velocity()[0];
/* External axes */
Vector3r airflow, lift_axis, drag_axis, side_axis;
real_t v;
airflow = attitude * (in.wind_velocity() - in.velocity());
v = airflow.norm();
if (v < DYNAMICS_MIN_V || v > DYNAMICS_MAX_V) {
/* Airflow too slow or too fast for any of this to work */
return AccelerationVector();
}
/*
Lift is always perpendicular to airflow, drag is always parallel, and
side is always towards the starboard wing of the aircraft.
*/
drag_axis = airflow / v;
side_axis = Vector3r(0, 1, 0);
lift_axis = drag_axis.cross(side_axis);
/* Determine alpha and beta: alpha = atan(wz/wx), beta = atan(wy/|wxz|) */
real_t alpha, beta,
v2_axial = airflow.x() * airflow.x() + airflow.z() * airflow.z(),
v2_side = airflow.x() * airflow.x() + airflow.y() * airflow.y(),
qbar_alpha = RHO * v2_axial * 0.5, qbar_beta = RHO * v2_side * 0.5;
alpha = atan2(-airflow.z(), -airflow.x()) / M_PI * 180.0;
beta = atan2(-airflow.y(), sqrt(v2_axial)) / M_PI * 180.0;
if (alpha < -20.0 || alpha > 20.0 || beta < -20.0 || beta > 20.0) {
/* Alpha or beta completely out of range */
return AccelerationVector();
}
/*
Set up a^4, a^3, a^2, a, 1 so that we can use dot product for the
polynomial evaluation.
*/
Eigen::Matrix<real_t, 5, 1> alpha_pow;
alpha_pow << pow(alpha, 4), pow(alpha, 3), pow(alpha, 2), alpha, 1.0;
/*
Determine control surface base force and moment contributions -- sum all
the alpha polynomial coefficients to minimise the number of dot products.
*/
Eigen::Matrix<real_t, 5, 1> cv_lift, cv_drag, cv_side, cv_yaw_moment,
cv_pitch_moment, cv_roll_moment;
cv_lift.setZero();
cv_drag.setZero();
cv_side.setZero();
cv_yaw_moment.setZero();
cv_pitch_moment.setZero();
cv_roll_moment.setZero();
for (int8_t i = control.rows() - 1; i >= 0; i--) {
if (i == motor_idx) {
continue;
}
real_t cv = control[i];
cv_lift += c_lift_alpha_cv.col(i) * cv;
cv_drag += c_drag_alpha_cv.col(i) * cv;
cv_side += c_side_alpha_cv.col(i) * cv;
cv_yaw_moment += c_yaw_moment_alpha_cv.col(i) * cv;
cv_pitch_moment += c_pitch_moment_alpha_cv.col(i) * cv;
cv_roll_moment += c_roll_moment_alpha_cv.col(i) * cv;
}
/* Find total lift, drag, side force and moment coefficient values */
real_t c_lift, c_drag, c_side, c_yaw_moment, c_pitch_moment,
c_roll_moment;
c_lift = alpha_pow.dot(c_lift_alpha + cv_lift) +
c_lift_pitch_rate * pitch_rate;
c_drag = alpha_pow.dot(c_drag_alpha + cv_drag);
c_side = alpha_pow.dot(
c_side_force_alpha_beta * beta +
c_side_force_alpha_roll_rate * roll_rate +
cv_side);
c_yaw_moment = alpha_pow.dot(
c_yaw_moment_alpha_beta * beta +
c_yaw_moment_alpha_roll_rate * roll_rate +
c_yaw_moment_alpha_yaw_rate * yaw_rate +
cv_yaw_moment);
c_roll_moment = alpha_pow.dot(
c_roll_moment_alpha_beta * beta +
c_roll_moment_alpha_roll_rate * roll_rate +
c_roll_moment_alpha_yaw_rate * yaw_rate +
cv_roll_moment);
c_pitch_moment = alpha_pow.dot(c_pitch_moment_alpha + cv_pitch_moment) +
c_pitch_moment_pitch_rate * pitch_rate;
/*
Sum and apply forces and moments based on aerodynamic reference point.
*/
Vector3r sum_force, sum_torque;
Vector3r lift, drag, side, yaw_moment, pitch_moment, roll_moment;
lift = c_lift * qbar_alpha * lift_axis;
drag = c_drag * qbar_alpha * drag_axis;
side = c_side * qbar_alpha * side_axis;
yaw_moment = c_yaw_moment * qbar_alpha * Vector3r(0, 0, 1);
pitch_moment = c_pitch_moment * qbar_alpha * Vector3r(0, 1, 0);
roll_moment = c_roll_moment * qbar_alpha * Vector3r(1, 0, 0);
sum_force = lift + drag + side;
sum_torque = lift.cross(aero_ref_pos) + drag.cross(aero_ref_pos) +
yaw_moment + pitch_moment + roll_moment;
/*
Determine motor thrust and torque:
https://www.grc.nasa.gov/WWW/Wright/airplane/propth.html
Propeller thrust =
F = 0.5 * rho * A * (Ve^2 - V0^2)
where A = propeller disc area, Ve = exit velocity, V0 = air velocity
In this formulation,
F = 0.5 * rho * A * ((k * rpm)^2 - V0^2)
Presumably the relationship between thrust and torque on airframe is
constant? If so, they're related by prop_ct.
*/
real_t thrust;
if (motor_idx < control.rows()) {
thrust = 0.5 * RHO * prop_area *
(pow(prop_cve * control[motor_idx], 2) - pow(airflow.x(), 2));
if (thrust < 0.0) {
/* Folding prop, so no drag */
thrust = 0.0;
}
/* Add motor thrust and torque to the net force */
sum_force += thrust * motor_thrust;
sum_torque += thrust * prop_ct * motor_moment;
}
/* Calculate linear acceleration (F / m) */
AccelerationVector output;
output.segment<3>(0) = sum_force * mass_inv +
attitude * Vector3r(0, 0, G_ACCEL);
/* Calculate angular acceleration (tau / inertia tensor) */
output.segment<3>(3) = inertia_tensor_inv * sum_torque;
return output;
}
The next step was obtaining values for those coefficients. Ideally, this would be done in a wind tunnel, but we don’t have access to one; thus, we had to make do with simulation and confirmation based on flight data logs.
Initially we used a model built in XFLR5 to generate polar plots at varying values of α and β. Unfortunately, I wasn’t able to obtain a particularly wide range of data due to some simulation issues with the winglets and airfoil, so the model wasn't as comprehensive as I would have liked, and in particular the stability derivatives (yaw, pitch and roll moments due to yaw, pitch and roll rate) were almost completely absent. In addition, XFLR5 doesn’t appear to support elevon simulation, so I wasn’t able to determine the control coefficients.
Plugging the very approximate coefficients obtained from XFLR5 into the flight dynamics model and running through the artificial horizon test case resulted in something of an improvement in attitude accuracy:
| Model | ° Error at Percentile | |||||||
|---|---|---|---|---|---|---|---|---|
| Pitch | Roll | |||||||
| 50th | 75th | 95th | RMS | 50th | 75th | 95th | RMS | |
| None | 4.0 | 6.6 | 13.4 | 6.5 | 4.5 | 7.8 | 14.6 | 7.3 |
| Centripetal | 2.6 | 4.4 | 9.3 | 5.4 | 3.0 | 5.4 | 12.8 | 7.7 |
| Fixed-wing | 2.2 | 3.7 | 7.2 | 4.3 | 2.7 | 4.7 | 8.3 | 6.1 |
While we’re still not at our minimum accuracy target of ±2° 75th percentile and ±5° 95th percentile error, we’re close enough that sensor alignment, calibration and vibration reduction should get us there.