// This is a Least Mean Squares adaptive filter
// It estiamtes the actuator effectiveness online by comparing the expected angular acceleration based on the inputs with the measured angular acceleration
static inline void lms_estimation(void)
{
static struct IndiEstimation *est = &indi.est;
// Only pass really low frequencies so you don't adapt to noise
stabilization_indi_second_order_filter(&est->u, &indi.u_act_dyn);
struct FloatRates *body_rates = stateGetBodyRates_f();
stabilization_indi_second_order_filter(&est->rate, body_rates);
// The inputs are scaled in order to avoid overflows
float du = est->u.dx.p * INDI_EST_SCALE;
est->g1.p = est->g1.p - (est->g1.p * du - est->rate.ddx.p) * du * est->mu;
du = est->u.dx.q * INDI_EST_SCALE;
est->g1.q = est->g1.q - (est->g1.q * du - est->rate.ddx.q) * du * est->mu;
du = est->u.dx.r * INDI_EST_SCALE;
float ddu = est->u.ddx.r * INDI_EST_SCALE / PERIODIC_FREQUENCY;
float error = (est->g1.r * du + est->g2 * ddu - est->rate.ddx.r);
est->g1.r = est->g1.r - error * du * est->mu / 3;
est->g2 = est->g2 - error * 1000 * ddu * est->mu / 3;
//the g values should be larger than zero, otherwise there is positive feedback, the command will go to max and there is nothing to learn anymore...
if (est->g1.p < 0.01) { est->g1.p = 0.01; }
if (est->g1.q < 0.01) { est->g1.q = 0.01; }
if (est->g1.r < 0.01) { est->g1.r = 0.01; }
if (est->g2 < 0.01) { est->g2 = 0.01; }
if (indi.adaptive) {
//Commit the estimated G values and apply the scaling
indi.g1.p = est->g1.p * INDI_EST_SCALE;
indi.g1.q = est->g1.q * INDI_EST_SCALE;
indi.g1.r = est->g1.r * INDI_EST_SCALE;
indi.g2 = est->g2 * INDI_EST_SCALE;
}
}
// This is a Least Mean Squares adaptive filter
// It estiamtes the actuator effectiveness online by comparing the expected angular acceleration based on the inputs with the measured angular acceleration
void lms_estimation(void)
{
// Only pass really low frequencies so you don't adapt to noise
float omega = 10.0;
float zeta = 0.8;
stabilization_indi_second_order_filter(&indi.u_act_dyn, &udotdot_estimation, &udot_estimation, &indi_u_estimation,
omega, zeta, omega);
struct FloatRates *body_rates = stateGetBodyRates_f();
stabilization_indi_second_order_filter(body_rates, &filtered_rate_2deriv_estimation, &filtered_rate_deriv_estimation,
&filtered_rate_estimation, omega, zeta, omega);
// The inputs are scaled in order to avoid overflows
float du = udot_estimation.p * SCALE;
g_est.p = g_est.p - (g_est.p * du - filtered_rate_2deriv_estimation.p) * du * mu;
du = udot_estimation.q * SCALE;
g_est.q = g_est.q - (g_est.q * du - filtered_rate_2deriv_estimation.q) * du * mu;
du = udot_estimation.r * SCALE;
float ddu = udotdot_estimation.r * SCALE / PERIODIC_FREQUENCY;
float error = (g_est.r * du + g2_est * ddu - filtered_rate_2deriv_estimation.r);
g_est.r = g_est.r - error * du * mu / 3;
g2_est = g2_est - error * 1000 * ddu * mu / 3;
//the g values should be larger than zero, otherwise there is positive feedback, the command will go to max and there is nothing to learn anymore...
if (g_est.p < 0.01) { g_est.p = 0.01; }
if (g_est.q < 0.01) { g_est.q = 0.01; }
if (g_est.r < 0.01) { g_est.r = 0.01; }
if (g2_est < 0.01) { g2_est = 0.01; }
//Commit the estimated G values and apply the scaling
g1.p = g_est.p * SCALE;
g1.q = g_est.q * SCALE;
g1.r = g_est.r * SCALE;
g2 = g2_est * SCALE;
}
void stabilization_attitude_run(bool_t enable_integrator)
{
/* Propagate the second order filter on the gyroscopes */
struct FloatRates *body_rates = stateGetBodyRates_f();
stabilization_indi_second_order_filter(body_rates, &indi.filtered_rate_2deriv, &indi.filtered_rate_deriv,
&indi.filtered_rate, STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);
/* attitude error */
struct Int32Quat att_err;
struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
// INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_sp_quat);
int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
/* wrap it in the shortest direction */
int32_quat_wrap_shortest(&att_err);
int32_quat_normalize(&att_err);
/* compute the INDI command */
attitude_run_indi(stabilization_att_indi_cmd, &att_err, enable_integrator);
stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL];
stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH];
stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_cmd[COMMAND_YAW];
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
static void attitude_run_indi(int32_t indi_commands[], struct Int32Quat *att_err, bool_t in_flight)
{
//Calculate required angular acceleration
struct FloatRates *body_rate = stateGetBodyRates_f();
indi.angular_accel_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
- reference_acceleration.rate_p * body_rate->p;
indi.angular_accel_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
- reference_acceleration.rate_q * body_rate->q;
indi.angular_accel_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
- reference_acceleration.rate_r * body_rate->r;
//Incremented in angular acceleration requires increment in control input
//G1 is the actuator effectiveness. In the yaw axis, we need something additional: G2.
//It takes care of the angular acceleration caused by the change in rotation rate of the propellers
//(they have significant inertia, see the paper mentioned in the header for more explanation)
indi.du.p = 1.0 / g1.p * (indi.angular_accel_ref.p - indi.filtered_rate_deriv.p);
indi.du.q = 1.0 / g1.q * (indi.angular_accel_ref.q - indi.filtered_rate_deriv.q);
indi.du.r = 1.0 / (g1.r + g2) * (indi.angular_accel_ref.r - indi.filtered_rate_deriv.r + g2 * indi.du.r);
//add the increment to the total control input
indi.u_in.p = indi.u.p + indi.du.p;
indi.u_in.q = indi.u.q + indi.du.q;
indi.u_in.r = indi.u.r + indi.du.r;
//bound the total control input
Bound(indi.u_in.p, -4500, 4500);
Bound(indi.u_in.q, -4500, 4500);
Bound(indi.u_in.r, -4500, 4500);
//Propagate input filters
//first order actuator dynamics
indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
//sensor filter
stabilization_indi_second_order_filter(&indi.u_act_dyn, &indi.udotdot, &indi.udot, &indi.u,
STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_ZETA, STABILIZATION_INDI_FILT_OMEGA_R);
//Don't increment if thrust is off
if (stabilization_cmd[COMMAND_THRUST] < 300) {
FLOAT_RATES_ZERO(indi.u);
FLOAT_RATES_ZERO(indi.du);
FLOAT_RATES_ZERO(indi.u_act_dyn);
FLOAT_RATES_ZERO(indi.u_in);
FLOAT_RATES_ZERO(indi.udot);
FLOAT_RATES_ZERO(indi.udotdot);
} else {
#if STABILIZATION_INDI_USE_ADAPTIVE
#warning "Use caution with adaptive indi. See the wiki for more info"
lms_estimation();
#endif
}
/* INDI feedback */
indi_commands[COMMAND_ROLL] = indi.u_in.p;
indi_commands[COMMAND_PITCH] = indi.u_in.q;
indi_commands[COMMAND_YAW] = indi.u_in.r;
}
static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct Int32Quat *att_err, bool rate_control)
{
/* Propagate the second order filter on the gyroscopes */
struct FloatRates *body_rates = stateGetBodyRates_f();
stabilization_indi_second_order_filter(&indi.rate, body_rates);
//The rates used for feedback are by default the measured rates. If needed they can be filtered (see below)
struct FloatRates rates_for_feedback;
RATES_COPY(rates_for_feedback, (*body_rates));
//If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
//Note that due to the delay, the PD controller can not be as aggressive.
#if STABILIZATION_INDI_FILTER_ROLL_RATE
rates_for_feedback.p = indi.rate.x.p;
#endif
#if STABILIZATION_INDI_FILTER_PITCH_RATE
rates_for_feedback.q = indi.rate.x.q;
#endif
#if STABILIZATION_INDI_FILTER_YAW_RATE
rates_for_feedback.r = indi.rate.x.r;
#endif
indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
- indi.reference_acceleration.rate_p * rates_for_feedback.p;
indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
- indi.reference_acceleration.rate_q * rates_for_feedback.q;
//This separates the P and D controller and lets you impose a maximum yaw rate.
float rate_ref_r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)/indi.reference_acceleration.rate_r;
BoundAbs(rate_ref_r, indi.attitude_max_yaw_rate);
indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (rate_ref_r - rates_for_feedback.r);
/* Check if we are running the rate controller and overwrite */
if(rate_control) {
indi.angular_accel_ref.p = indi.reference_acceleration.rate_p * ((float)radio_control.values[RADIO_ROLL] / MAX_PPRZ * indi.max_rate - body_rates->p);
indi.angular_accel_ref.q = indi.reference_acceleration.rate_q * ((float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * indi.max_rate - body_rates->q);
indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * ((float)radio_control.values[RADIO_YAW] / MAX_PPRZ * indi.max_rate - body_rates->r);
}
//Increment in angular acceleration requires increment in control input
//G1 is the control effectiveness. In the yaw axis, we need something additional: G2.
//It takes care of the angular acceleration caused by the change in rotation rate of the propellers
//(they have significant inertia, see the paper mentioned in the header for more explanation)
indi.du.p = 1.0 / indi.g1.p * (indi.angular_accel_ref.p - indi.rate.dx.p);
indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate.dx.q);
indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate.dx.r + indi.g2 * indi.du.r);
//add the increment to the total control input
indi.u_in.p = indi.u.x.p + indi.du.p;
indi.u_in.q = indi.u.x.q + indi.du.q;
indi.u_in.r = indi.u.x.r + indi.du.r;
//bound the total control input
Bound(indi.u_in.p, -4500, 4500);
Bound(indi.u_in.q, -4500, 4500);
Bound(indi.u_in.r, -4500, 4500);
//Propagate input filters
//first order actuator dynamics
indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
//sensor filter
stabilization_indi_second_order_filter(&indi.u, &indi.u_act_dyn);
//Don't increment if thrust is off
//TODO: this should be something more elegant, but without this the inputs will increment to the maximum before
//even getting in the air.
if (stabilization_cmd[COMMAND_THRUST] < 300) {
FLOAT_RATES_ZERO(indi.du);
FLOAT_RATES_ZERO(indi.u_act_dyn);
FLOAT_RATES_ZERO(indi.u_in);
FLOAT_RATES_ZERO(indi.u.x);
FLOAT_RATES_ZERO(indi.u.dx);
FLOAT_RATES_ZERO(indi.u.ddx);
} else {
// only run the estimation if the commands are not zero.
lms_estimation();
}
/* INDI feedback */
indi_commands[COMMAND_ROLL] = indi.u_in.p;
indi_commands[COMMAND_PITCH] = indi.u_in.q;
indi_commands[COMMAND_YAW] = indi.u_in.r;
}
static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct Int32Quat *att_err, bool_t rate_control)
{
/* Propagate the second order filter on the gyroscopes */
struct FloatRates *body_rates = stateGetBodyRates_f();
stabilization_indi_second_order_filter(&indi.rate, body_rates);
#if STABILIZATION_INDI_FILTER_ROLL_RATE
indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
- indi.reference_acceleration.rate_p * indi.rate.x.p;
#else
indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
- indi.reference_acceleration.rate_p * body_rates->p;
#endif
#if STABILIZATION_INDI_FILTER_PITCH_RATE
indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
- indi.reference_acceleration.rate_q * indi.rate.x.q;
#else
indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
- indi.reference_acceleration.rate_q * body_rates->q;
#endif
#if STABILIZATION_INDI_FILTER_YAW_RATE
indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
- indi.reference_acceleration.rate_r * indi.rate.x.r;
#else
indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
- indi.reference_acceleration.rate_r * body_rates->r;
#endif
/* Check if we are running the rate controller and overwrite */
if(rate_control) {
indi.angular_accel_ref.p = indi.reference_acceleration.rate_p * ((float)radio_control.values[RADIO_ROLL] / MAX_PPRZ * indi.max_rate - body_rates->p);
indi.angular_accel_ref.q = indi.reference_acceleration.rate_q * ((float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * indi.max_rate - body_rates->q);
indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * ((float)radio_control.values[RADIO_YAW] / MAX_PPRZ * indi.max_rate - body_rates->r);
}
//Incremented in angular acceleration requires increment in control input
//G1 is the actuator effectiveness. In the yaw axis, we need something additional: G2.
//It takes care of the angular acceleration caused by the change in rotation rate of the propellers
//(they have significant inertia, see the paper mentioned in the header for more explanation)
indi.du.p = 1.0 / indi.g1.p * (indi.angular_accel_ref.p - indi.rate.dx.p);
indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate.dx.q);
indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate.dx.r + indi.g2 * indi.du.r);
//add the increment to the total control input
indi.u_in.p = indi.u.x.p + indi.du.p;
indi.u_in.q = indi.u.x.q + indi.du.q;
indi.u_in.r = indi.u.x.r + indi.du.r;
//bound the total control input
Bound(indi.u_in.p, -4500, 4500);
Bound(indi.u_in.q, -4500, 4500);
Bound(indi.u_in.r, -4500, 4500);
//Propagate input filters
//first order actuator dynamics
indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
//sensor filter
stabilization_indi_second_order_filter(&indi.u, &indi.u_act_dyn);
//Don't increment if thrust is off
if (stabilization_cmd[COMMAND_THRUST] < 300) {
FLOAT_RATES_ZERO(indi.du);
FLOAT_RATES_ZERO(indi.u_act_dyn);
FLOAT_RATES_ZERO(indi.u_in);
FLOAT_RATES_ZERO(indi.u.x);
FLOAT_RATES_ZERO(indi.u.dx);
FLOAT_RATES_ZERO(indi.u.ddx);
} else {
lms_estimation();
}
/* INDI feedback */
indi_commands[COMMAND_ROLL] = indi.u_in.p;
indi_commands[COMMAND_PITCH] = indi.u_in.q;
indi_commands[COMMAND_YAW] = indi.u_in.r;
}
