mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-10-02 12:23:17 +08:00
[module] wind estimation from quadrotor motion (#2800)
- add a generic linear kalman filter lib - add a quad model with linear drag and simplified for recent jsbsim - add example frame and noisy NPS sensor config see "Estimating wind using a quadrotor" in IMAV2021 proceedings
This commit is contained in:
Gautier Hattenberger
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,50 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
<module name="wind_estimation_quadrotor" dir="meteo">
|
||||
<doc>
|
||||
<description>
|
||||
Wind estimation from quadrotor motion
|
||||
Estimation using a linear Kalman filter
|
||||
|
||||
For details, see:
|
||||
G. Hattenberger, M. Bronz, and J. Condomines, “Estimating wind using a quadrotor,”
|
||||
in 12th international micro air vehicle conference, Puebla, México, 2021, p. 124–130.
|
||||
</description>
|
||||
<section name="WE_QUAD" prefix="WE_QUAD_">
|
||||
<define name="MASS" value="required" description="mass of the airframe" unit="kg"/>
|
||||
<define name="DRAG" value="required" description="linear drag coefficient modeling the relation between drag and bank angle"/>
|
||||
<define name="P0_VA" value="1." description="initial covariance on airspeed estimate"/>
|
||||
<define name="P0_W" value="1." description="initial covariance on wind estimate"/>
|
||||
<define name="Q_VA" value="0.05" description="process noise on airspeed"/>
|
||||
<define name="Q_W" value="0.001" description="process noise on wind estimate"/>
|
||||
<define name="R" value="0.5" description="measurement noise on ground speed"/>
|
||||
<define name="UPDATE_STATE" value="FALSE|TRUE" description="update directly wind estimation in state interface (default: TRUE)"/>
|
||||
</section>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="Wind quad">
|
||||
<dl_setting MIN="0.001" MAX="1" STEP="0.01" VAR="we_quad_params.Q_va" shortname="Q_va" module="meteo/wind_estimation_quadrotor" handler="SetQva"/>
|
||||
<dl_setting MIN="0.001" MAX="0.1" STEP="0.001" VAR="we_quad_params.Q_w" shortname="Q_w" module="meteo/wind_estimation_quadrotor" handler="SetQw"/>
|
||||
<dl_setting MIN="0.01" MAX="3" STEP="0.01" VAR="we_quad_params.R" shortname="R" module="meteo/wind_estimation_quadrotor" handler="SetR"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="wind_estimation_quadrotor.h"/>
|
||||
</header>
|
||||
<init fun="wind_estimation_quadrotor_init()"/>
|
||||
<periodic fun="wind_estimation_quadrotor_periodic()" freq="10.0" start="wind_estimation_quadrotor_stop()" stop="wind_estimation_quadrotor_start()" autorun="FALSE"/>
|
||||
<periodic fun="wind_estimation_quadrotor_report()" freq="10.0" autorun="FALSE"/>
|
||||
<makefile>
|
||||
<file name="wind_estimation_quadrotor.c"/>
|
||||
<file name="linear_kalman_filter.c" dir="filters"/>
|
||||
<test>
|
||||
<define name="WE_QUAD_MASS" value="1."/>
|
||||
<define name="WE_QUAD_DRAG" value="1."/>
|
||||
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
|
||||
<define name="DOWNLINK_DEVICE" value="uart0"/>
|
||||
<define name="USE_UART0"/>
|
||||
<define name="WIND_ESTIMATION_QUADROTOR_PERIODIC_PERIOD" value="0.1"/>
|
||||
</test>
|
||||
</makefile>
|
||||
</module>
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,194 @@
|
||||
/*
|
||||
* Copyright (C) 2012 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
#ifndef NPS_SENSORS_PARAMS_H
|
||||
#define NPS_SENSORS_PARAMS_H
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "modules/imu/imu.h"
|
||||
|
||||
|
||||
#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI
|
||||
#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA
|
||||
#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI
|
||||
|
||||
// try to determine propagate frequency
|
||||
#if defined AHRS_PROPAGATE_FREQUENCY
|
||||
#define NPS_PROPAGATE AHRS_PROPAGATE_FREQUENCY
|
||||
#elif defined INS_PROPAGATE_FREQUENCY
|
||||
#define NPS_PROPAGATE INS_PROPAGATE_FREQUENCY
|
||||
#elif defined PERIODIC_FREQUENCY
|
||||
#define NPS_PROPAGATE PERIODIC_FREQUENCY
|
||||
#else
|
||||
#define 512. // historical magic number
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Accelerometer
|
||||
*/
|
||||
/* assume resolution is less than 16 bits, so saturation will not occur */
|
||||
#ifndef NPS_ACCEL_MIN
|
||||
#define NPS_ACCEL_MIN -65536
|
||||
#endif
|
||||
#ifndef NPS_ACCEL_MAX
|
||||
#define NPS_ACCEL_MAX 65536
|
||||
#endif
|
||||
/* ms-2 */
|
||||
/* aka 2^10/ACCEL_X_SENS */
|
||||
#define NPS_ACCEL_SENSITIVITY_XX (IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS))
|
||||
#define NPS_ACCEL_SENSITIVITY_YY (IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS))
|
||||
#define NPS_ACCEL_SENSITIVITY_ZZ (IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS))
|
||||
|
||||
#define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL
|
||||
#define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL
|
||||
#define NPS_ACCEL_NEUTRAL_Z IMU_ACCEL_Z_NEUTRAL
|
||||
/* m2s-4 */
|
||||
#define NPS_ACCEL_NOISE_STD_DEV_X .5
|
||||
#define NPS_ACCEL_NOISE_STD_DEV_Y .5
|
||||
#define NPS_ACCEL_NOISE_STD_DEV_Z .5
|
||||
/* ms-2 */
|
||||
#define NPS_ACCEL_BIAS_X 0
|
||||
#define NPS_ACCEL_BIAS_Y 0
|
||||
#define NPS_ACCEL_BIAS_Z 0
|
||||
/* s */
|
||||
#ifndef NPS_ACCEL_DT
|
||||
#define NPS_ACCEL_DT (1./NPS_PROPAGATE)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Gyrometer
|
||||
*/
|
||||
/* assume resolution is less than 16 bits, so saturation will not occur */
|
||||
#ifndef NPS_GYRO_MIN
|
||||
#define NPS_GYRO_MIN -65536
|
||||
#endif
|
||||
#ifndef NPS_GYRO_MAX
|
||||
#define NPS_GYRO_MAX 65536
|
||||
#endif
|
||||
|
||||
/* 2^12/GYRO_X_SENS */
|
||||
#define NPS_GYRO_SENSITIVITY_PP (IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS))
|
||||
#define NPS_GYRO_SENSITIVITY_QQ (IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS))
|
||||
#define NPS_GYRO_SENSITIVITY_RR (IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS))
|
||||
|
||||
#define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL
|
||||
#define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL
|
||||
#define NPS_GYRO_NEUTRAL_R IMU_GYRO_R_NEUTRAL
|
||||
|
||||
#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(9.)
|
||||
#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(9.)
|
||||
#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(9.)
|
||||
|
||||
#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
|
||||
#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0)
|
||||
#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0)
|
||||
|
||||
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
|
||||
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
|
||||
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5)
|
||||
/* s */
|
||||
#ifndef NPS_GYRO_DT
|
||||
#define NPS_GYRO_DT (1./NPS_PROPAGATE)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Magnetometer
|
||||
*/
|
||||
/* assume resolution is less than 16 bits, so saturation will not occur */
|
||||
#ifndef NPS_MAG_MIN
|
||||
#define NPS_MAG_MIN -65536
|
||||
#endif
|
||||
#ifndef NPS_MAG_MAX
|
||||
#define NPS_MAG_MAX 65536
|
||||
#endif
|
||||
|
||||
#define NPS_MAG_IMU_TO_SENSOR_PHI 0.
|
||||
#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
|
||||
#define NPS_MAG_IMU_TO_SENSOR_PSI 0.
|
||||
|
||||
#define NPS_MAG_SENSITIVITY_XX (IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS))
|
||||
#define NPS_MAG_SENSITIVITY_YY (IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS))
|
||||
#define NPS_MAG_SENSITIVITY_ZZ (IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS))
|
||||
|
||||
#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL
|
||||
#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL
|
||||
#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL
|
||||
|
||||
#define NPS_MAG_NOISE_STD_DEV_X 2e-1
|
||||
#define NPS_MAG_NOISE_STD_DEV_Y 2e-1
|
||||
#define NPS_MAG_NOISE_STD_DEV_Z 2e-1
|
||||
|
||||
#ifndef NPS_MAG_DT
|
||||
#define NPS_MAG_DT (1./100.)
|
||||
#endif
|
||||
|
||||
|
||||
/*
|
||||
* Barometer (pressure and std dev in Pascal)
|
||||
*/
|
||||
#define NPS_BARO_DT (1./50.)
|
||||
#define NPS_BARO_NOISE_STD_DEV 2
|
||||
|
||||
/*
|
||||
* GPS
|
||||
*/
|
||||
|
||||
#ifndef GPS_PERFECT
|
||||
#define GPS_PERFECT 0
|
||||
#endif
|
||||
|
||||
#if GPS_PERFECT
|
||||
|
||||
#define NPS_GPS_SPEED_NOISE_STD_DEV 0.
|
||||
#define NPS_GPS_SPEED_LATENCY 0.
|
||||
#define NPS_GPS_POS_NOISE_STD_DEV 0.001
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_X 0.
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_Y 0.
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_Z 0.
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
|
||||
#define NPS_GPS_POS_LATENCY 0.
|
||||
|
||||
#else
|
||||
|
||||
#define NPS_GPS_SPEED_NOISE_STD_DEV 0.5
|
||||
#define NPS_GPS_SPEED_LATENCY 0.2
|
||||
#define NPS_GPS_POS_NOISE_STD_DEV 2
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_X 0e-1
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_Y -0e-1
|
||||
#define NPS_GPS_POS_BIAS_INITIAL_Z -0e-1
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
|
||||
#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
|
||||
#define NPS_GPS_POS_LATENCY 0.2
|
||||
|
||||
#endif /* GPS_PERFECT */
|
||||
|
||||
#ifndef NPS_GPS_DT
|
||||
#define NPS_GPS_DT (1./10.)
|
||||
#endif
|
||||
|
||||
#endif /* NPS_SENSORS_PARAMS_H */
|
||||
@@ -35,6 +35,7 @@
|
||||
<message name="LOGGER_STATUS" period="5.1"/>
|
||||
<message name="LIDAR" period="1.2"/>
|
||||
<message name="INS_EKF2" period=".25"/>
|
||||
<message name="WIND_INFO_RET" period="1."/>
|
||||
</mode>
|
||||
|
||||
<mode name="ppm">
|
||||
|
||||
@@ -0,0 +1,149 @@
|
||||
/*
|
||||
* Copyright (C) 2021 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/>.
|
||||
*/
|
||||
/**
|
||||
* @file "filters/linear_kalman_filter.c"
|
||||
*
|
||||
* Generic discrete Linear Kalman Filter
|
||||
*/
|
||||
|
||||
#include "filters/linear_kalman_filter.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
|
||||
/** Init all matrix and vectors to zero
|
||||
*
|
||||
* @param filter pointer to a filter structure
|
||||
* @param n size of the state vector
|
||||
* @param c size of the command vector
|
||||
* @param m size of the measurement vector
|
||||
* @return false if n, c or m are larger than the maximum value
|
||||
*/
|
||||
bool linear_kalman_filter_init(struct linear_kalman_filter *filter, uint8_t n, uint8_t c, uint8_t m)
|
||||
{
|
||||
if (n > KF_MAX_STATE_SIZE || c > KF_MAX_CMD_SIZE || m > KF_MAX_MEAS_SIZE) {
|
||||
filter->n = 0;
|
||||
filter->c = 0;
|
||||
filter->m = 0;
|
||||
return false; // invalide sizes;
|
||||
}
|
||||
filter->n = n;
|
||||
filter->c = c;
|
||||
filter->m = m;
|
||||
|
||||
// Matrix
|
||||
MAKE_MATRIX_PTR(_A, filter->A, n);
|
||||
float_mat_zero(_A, n, n);
|
||||
MAKE_MATRIX_PTR(_B, filter->B, n);
|
||||
float_mat_zero(_B, n, c);
|
||||
MAKE_MATRIX_PTR(_C, filter->C, m);
|
||||
float_mat_zero(_C, m, n);
|
||||
MAKE_MATRIX_PTR(_P, filter->P, n);
|
||||
float_mat_zero(_P, n, n);
|
||||
MAKE_MATRIX_PTR(_Q, filter->Q, n);
|
||||
float_mat_zero(_Q, n, n);
|
||||
MAKE_MATRIX_PTR(_R, filter->R, m);
|
||||
float_mat_zero(_R, m, m);
|
||||
|
||||
// Vector
|
||||
float_vect_zero(filter->X, n);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/** Prediction step
|
||||
*
|
||||
* X = Ad * X + Bd * U
|
||||
* P = Ad * P * Ad' + Q
|
||||
*
|
||||
* @param filter pointer to the filter structure
|
||||
* @param U command vector
|
||||
*/
|
||||
void linear_kalman_filter_predict(struct linear_kalman_filter *filter, float *U)
|
||||
{
|
||||
float AX[filter->n];
|
||||
float BU[filter->n];
|
||||
float tmp[filter->n][filter->n];
|
||||
|
||||
MAKE_MATRIX_PTR(_A, filter->A, filter->n);
|
||||
MAKE_MATRIX_PTR(_B, filter->B, filter->n);
|
||||
MAKE_MATRIX_PTR(_P, filter->P, filter->n);
|
||||
MAKE_MATRIX_PTR(_Q, filter->Q, filter->n);
|
||||
MAKE_MATRIX_PTR(_tmp, tmp, filter->n);
|
||||
|
||||
// X = A * X + B * U
|
||||
float_mat_vect_mul(AX, _A, filter->X, filter->n, filter->n);
|
||||
float_mat_vect_mul(BU, _B, U, filter->n, filter->c);
|
||||
float_vect_sum(filter->X, AX, BU, filter->n);
|
||||
|
||||
// P = A * P * A' + Q
|
||||
float_mat_mul(_tmp, _A, _P, filter->n, filter->n, filter->n); // A * P
|
||||
float_mat_mul_transpose(_P, _tmp, _A, filter->n, filter->n, filter->n); // * A'
|
||||
float_mat_sum(_P, _P, _Q, filter->n, filter->n); // + Q
|
||||
}
|
||||
|
||||
/** Update step
|
||||
*
|
||||
* S = Cd * P * Cd' + R
|
||||
* K = P * Cd' / S
|
||||
* X = X + K * (Y - Cd * X)
|
||||
* P = P - K * Cd * P
|
||||
*
|
||||
* @param filter pointer to the filter structure
|
||||
* @param Y measurement vector
|
||||
*/
|
||||
extern void linear_kalman_filter_update(struct linear_kalman_filter *filter, float *Y)
|
||||
{
|
||||
float S[filter->m][filter->m];
|
||||
float K[filter->n][filter->m];
|
||||
float tmp1[filter->n][filter->m];
|
||||
float tmp2[filter->n][filter->n];
|
||||
|
||||
MAKE_MATRIX_PTR(_P, filter->P, filter->n);
|
||||
MAKE_MATRIX_PTR(_C, filter->C, filter->m);
|
||||
MAKE_MATRIX_PTR(_R, filter->R, filter->m);
|
||||
MAKE_MATRIX_PTR(_S, S, filter->m);
|
||||
MAKE_MATRIX_PTR(_K, K, filter->n);
|
||||
MAKE_MATRIX_PTR(_tmp1, tmp1, filter->n);
|
||||
MAKE_MATRIX_PTR(_tmp2, tmp2, filter->n);
|
||||
|
||||
// S = Cd * P * Cd' + R
|
||||
float_mat_mul_transpose(_tmp1, _P, _C, filter->n, filter->n, filter->m); // P * C'
|
||||
float_mat_mul(_S, _C, _tmp1, filter->m, filter->n, filter->m); // C *
|
||||
float_mat_sum(_S, _S, _R, filter->m, filter->m); // + R
|
||||
|
||||
// K = P * Cd' * inv(S)
|
||||
float_mat_invert(_S, _S, filter->m); // inv(S) in place
|
||||
float_mat_mul(_K, _tmp1, _S, filter->n, filter->m, filter->m); // tmp1 {P*C'} * inv(S)
|
||||
|
||||
// P = P - K * C * P
|
||||
float_mat_mul(_tmp2, _K, _C, filter->n, filter->m, filter->n); // K * C
|
||||
float_mat_mul_copy(_tmp2, _tmp2, _P, filter->n, filter->n, filter->n); // * P
|
||||
float_mat_diff(_P, _P, _tmp2, filter->n, filter->n); // P - K*H*P
|
||||
|
||||
// X = X + K * err
|
||||
float err[filter->n];
|
||||
float dx_err[filter->n];
|
||||
|
||||
float_mat_vect_mul(err, _C, filter->X, filter->m, filter->n); // C * X
|
||||
float_vect_diff(err, Y, err, filter->m); // err = Y - C * X
|
||||
float_mat_vect_mul(dx_err, _K, err, filter->n, filter->m); // K * err
|
||||
float_vect_sum(filter->X, filter->X, dx_err, filter->n); // X + dx_err
|
||||
}
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
* Copyright (C) 2021 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/>.
|
||||
*/
|
||||
/**
|
||||
* @file "filters/linear_kalman_filter.c"
|
||||
*
|
||||
* Generic discrete Linear Kalman Filter
|
||||
*/
|
||||
|
||||
#ifndef LINEAR_KALMAN_FILTER_H
|
||||
#define LINEAR_KALMAN_FILTER_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
// maximum size for the state vector
|
||||
#ifndef KF_MAX_STATE_SIZE
|
||||
#define KF_MAX_STATE_SIZE 6
|
||||
#endif
|
||||
|
||||
// maximum size for the command vector
|
||||
#ifndef KF_MAX_CMD_SIZE
|
||||
#define KF_MAX_CMD_SIZE 2
|
||||
#endif
|
||||
|
||||
// maximum size for the measurement vector
|
||||
#ifndef KF_MAX_MEAS_SIZE
|
||||
#define KF_MAX_MEAS_SIZE 6
|
||||
#endif
|
||||
|
||||
struct linear_kalman_filter {
|
||||
// filled by user after calling init function
|
||||
float A[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]; ///< dynamic matrix
|
||||
float B[KF_MAX_STATE_SIZE][KF_MAX_CMD_SIZE]; ///< command matrix
|
||||
float C[KF_MAX_MEAS_SIZE][KF_MAX_STATE_SIZE]; ///< observation matrix
|
||||
float P[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]; ///< state covariance matrix
|
||||
float Q[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]; ///< proces covariance noise
|
||||
float R[KF_MAX_MEAS_SIZE][KF_MAX_MEAS_SIZE]; ///< measurement covariance noise
|
||||
|
||||
float X[KF_MAX_STATE_SIZE]; ///< estimated state X
|
||||
|
||||
uint8_t n; ///< state vector size (<= KF_MAX_STATE_SIZE)
|
||||
uint8_t c; ///< command vector size (<= KF_MAX_CMD_SIZE)
|
||||
uint8_t m; ///< measurement vector size (<= KF_MAX_MEAS_SIZE)
|
||||
};
|
||||
|
||||
/** Init all matrix and vectors to zero
|
||||
*
|
||||
* @param filter pointer to a filter structure
|
||||
* @param n size of the state vector
|
||||
* @param c size of the command vector
|
||||
* @param m size of the measurement vector
|
||||
* @return false if n, c or m are larger than the maximum value
|
||||
*/
|
||||
extern bool linear_kalman_filter_init(struct linear_kalman_filter *filter, uint8_t n, uint8_t c, uint8_t m);
|
||||
|
||||
/** Prediction step
|
||||
*
|
||||
* X = Ad * X + Bd * U
|
||||
* P = Ad * P * Ad' + Q
|
||||
*
|
||||
* @param filter pointer to the filter structure
|
||||
* @param U command vector
|
||||
*/
|
||||
extern void linear_kalman_filter_predict(struct linear_kalman_filter *filter, float *U);
|
||||
|
||||
/** Update step
|
||||
*
|
||||
* S = Cd * P * Cd' + R
|
||||
* K = P * Cd' / S
|
||||
* X = X + K * (Y - Cd * X)
|
||||
* P = P - K * Cd * P
|
||||
*
|
||||
* @param filter pointer to the filter structure
|
||||
* @param Y measurement vector
|
||||
*/
|
||||
extern void linear_kalman_filter_update(struct linear_kalman_filter *filter, float *Y);
|
||||
|
||||
#endif /* DISCRETE_EKF_H */
|
||||
@@ -728,6 +728,25 @@ static inline void float_mat_mul(float **o, float **a, float **b, int m, int n,
|
||||
}
|
||||
}
|
||||
|
||||
/** o = a * b'
|
||||
*
|
||||
* a: [m x n]
|
||||
* b: [l x n]
|
||||
* o: [m x l]
|
||||
*/
|
||||
static inline void float_mat_mul_transpose(float **o, float **a, float **b, int m, int n, int l)
|
||||
{
|
||||
int i, j, k;
|
||||
for (i = 0; i < m; i++) {
|
||||
for (j = 0; j < l; j++) {
|
||||
o[i][j] = 0.;
|
||||
for (k = 0; k < n; k++) {
|
||||
o[i][j] += a[i][k] * b[j][k];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** o = a * b
|
||||
*
|
||||
* a: [m x n]
|
||||
|
||||
@@ -327,15 +327,9 @@ float get_tas_factor(float p, float t)
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from equivalent airspeed.
|
||||
*
|
||||
* True airspeed (TAS) from EAS:
|
||||
* TAS = air_data.tas_factor * EAS
|
||||
*
|
||||
* @param eas equivalent airspeed (EAS) in m/s
|
||||
* @return true airspeed in m/s
|
||||
* Internal utility function to compute current tas factor if needed
|
||||
*/
|
||||
float tas_from_eas(float eas)
|
||||
static void compute_tas_factor(void)
|
||||
{
|
||||
// update tas factor if requested
|
||||
if (air_data.calc_tas_factor) {
|
||||
@@ -351,9 +345,38 @@ float tas_from_eas(float eas)
|
||||
air_data.tas_factor = get_tas_factor(p, CelsiusOfKelvin(t));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from equivalent airspeed.
|
||||
*
|
||||
* True airspeed (TAS) from EAS:
|
||||
* TAS = air_data.tas_factor * EAS
|
||||
*
|
||||
* @param eas equivalent airspeed (EAS) in m/s
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
float tas_from_eas(float eas)
|
||||
{
|
||||
compute_tas_factor();
|
||||
return air_data.tas_factor * eas;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate equivalent airspeed from true airspeed.
|
||||
*
|
||||
* EAS from True airspeed (TAS):
|
||||
* EAS = TAS / air_data.tas_factor
|
||||
*
|
||||
* @param tas true airspeed (TAS) in m/s
|
||||
* @return equivalent airspeed in m/s
|
||||
*/
|
||||
float eas_from_tas(float tas)
|
||||
{
|
||||
compute_tas_factor();
|
||||
return tas / air_data.tas_factor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from dynamic pressure.
|
||||
* Dynamic pressure @f$q@f$ (also called impact pressure) is the
|
||||
|
||||
@@ -101,6 +101,14 @@ extern float get_tas_factor(float p, float t);
|
||||
*/
|
||||
extern float tas_from_eas(float eas);
|
||||
|
||||
/**
|
||||
* Calculate equivalent airspeed from true airspeed.
|
||||
*
|
||||
* @param tas true airspeed (TAS) in m/s
|
||||
* @return equivalent airspeed in m/s
|
||||
*/
|
||||
extern float eas_from_tas(float tas);
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from dynamic pressure.
|
||||
* Dynamic pressure @f$q@f$ (also called impact pressure) is the
|
||||
|
||||
@@ -111,6 +111,10 @@
|
||||
#define AIRSPEED_ETS_ID 4
|
||||
#endif
|
||||
|
||||
#ifndef AIRSPEED_WE_QUAD_ID
|
||||
#define AIRSPEED_WE_QUAD_ID 5
|
||||
#endif
|
||||
|
||||
/*
|
||||
* IDs of Incidence angles (message 24)
|
||||
*/
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,47 @@
|
||||
/*
|
||||
* Copyright (C) 2021 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/>.
|
||||
*/
|
||||
|
||||
/** @file "modules/meteo/wind_estimation_quadrotor.h"
|
||||
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Wind estimation from quadrotor motion
|
||||
*/
|
||||
|
||||
#ifndef WIND_ESTIMATION_QUADROTOR_H
|
||||
#define WIND_ESTIMATION_QUADROTOR_H
|
||||
|
||||
struct wind_estimation_quadrotor_params {
|
||||
float Q_va; ///< model noise on airspeed
|
||||
float Q_w; ///< model noise on wind
|
||||
float R; ///< measurement noise (ground speed)
|
||||
};
|
||||
|
||||
extern struct wind_estimation_quadrotor_params we_quad_params;
|
||||
|
||||
extern void wind_estimation_quadrotor_init(void);
|
||||
extern void wind_estimation_quadrotor_periodic(void);
|
||||
extern void wind_estimation_quadrotor_stop(void);
|
||||
extern void wind_estimation_quadrotor_start(void);
|
||||
extern void wind_estimation_quadrotor_report(void);
|
||||
|
||||
extern float wind_estimation_quadrotor_SetQva(float Q_va);
|
||||
extern float wind_estimation_quadrotor_SetQw(float Q_w);
|
||||
extern float wind_estimation_quadrotor_SetR(float R);
|
||||
|
||||
#endif // WIND_ESTIMATION_QUADROTOR_H
|
||||
@@ -576,6 +576,7 @@ static void init_jsbsim(double dt)
|
||||
delete FDMExec;
|
||||
exit(-1);
|
||||
}
|
||||
cout << "JSBSim model loaded from " << NPS_JSBSIM_MODEL << endl;
|
||||
|
||||
#ifdef DEBUG
|
||||
cerr << "NumEngines: " << FDMExec->GetPropulsion()->GetNumEngines() << endl;
|
||||
|
||||
Reference in New Issue
Block a user