quaternion_filter.h

Go to the documentation of this file.

/*
 * Copyright (C) 2026 Gautier Hattenberger <gautier.hattenberger@enac.fr>
 *
 * This file is part of paparazzi
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 *
 */
 
#ifndef QUATERNION_FILTER_H
#define QUATERNION_FILTER_H
 
#include "std.h"
#include "math/pprz_algebra_float.h"
 
struct QuatSecondOrderLowPass {
  struct FloatQuat  quat;
  struct FloatRates rate;
  struct FloatRates accel;
 
  float two_zeta_omega;
  float two_omega2;
  float dt;
};
 
static inline void init_quat_second_order_low_pass(
    struct QuatSecondOrderLowPass *filter,
    const float omega,
    const float damp,
    const float sample_time,
    const struct FloatQuat q0)
{
  filter->quat = q0;
  FLOAT_RATES_ZERO(filter->rate);
  FLOAT_RATES_ZERO(filter->accel);
  filter->two_zeta_omega = 2.f * damp * omega;
  filter->two_omega2 = 2.f * omega * omega;
  filter->dt = sample_time;
}
 
static inline void reset_quat_second_order_low_pass(
    struct QuatSecondOrderLowPass *filter,
    const struct FloatQuat q0)
{
  filter->quat = q0;
  FLOAT_RATES_ZERO(filter->rate);
  FLOAT_RATES_ZERO(filter->accel);
}
 
static inline struct FloatQuat update_quat_second_order_low_pass(
    struct QuatSecondOrderLowPass *filter,
    const struct FloatQuat q)
{
  /* integrate quaternion */
  struct FloatQuat qdot;
  float_quat_derivative(&qdot, &filter->rate, &filter->quat);
  QUAT_SMUL(qdot, qdot, filter->dt);
  QUAT_ADD(filter->quat, qdot);
  float_quat_normalize(&filter->quat);
 
  /* integrate rotational speed */
  struct FloatRates delta_rate;
  RATES_SMUL(delta_rate, filter->accel, filter->dt);
  RATES_ADD(filter->rate, delta_rate);
 
  /* compute angular accelerations */
  struct FloatQuat err;
  /* attitude error */
  float_quat_inv_comp(&err, &q, &filter->quat);
  /* wrap it in the shortest direction */
  float_quat_wrap_shortest(&err);
  /* propagate the 2nd order linear model: xdotdot = -2*zeta*omega*xdot - omega^2*x */
  /* since error quaternion contains the half-angles we get 2*omega^2*err */
  filter->accel.p = -filter->two_zeta_omega * filter->rate.p - filter->two_omega2 * err.qx;
  filter->accel.q = -filter->two_zeta_omega * filter->rate.q - filter->two_omega2 * err.qy;
  filter->accel.r = -filter->two_zeta_omega * filter->rate.r - filter->two_omega2 * err.qz;
 
  return filter->quat;
}
 
static inline struct FloatQuat get_quat_second_order_low_pass(
    const struct QuatSecondOrderLowPass *filter)
{
  return filter->quat;
}
 
 
typedef struct QuatSecondOrderLowPass QuatButterworthLowPass;
 
static inline void init_quat_butterworth_low_pass(
    QuatButterworthLowPass *filter,
    const float omega,
    const float sample_time,
    const struct FloatQuat q0)
{
  init_quat_second_order_low_pass((struct QuatSecondOrderLowPass *)filter, omega, 0.7071f, sample_time, q0);
}
 
static inline struct FloatQuat update_quat_butterworth_low_pass(
    QuatButterworthLowPass *filter,
    const struct FloatQuat quat)
{
  return update_quat_second_order_low_pass((struct QuatSecondOrderLowPass *)filter, quat);
}
 
static inline void reset_quat_butterworth_low_pass(
    QuatButterworthLowPass *filter,
    const struct FloatQuat quat)
{
  reset_quat_second_order_low_pass((struct QuatSecondOrderLowPass *)filter, quat);
}
 
static inline struct FloatQuat get_quat_butterworth_low_pass(
    const QuatButterworthLowPass *filter)
{
  return get_quat_second_order_low_pass((const struct QuatSecondOrderLowPass *)filter);
}
 
#endif