diff --git a/conf/airframes/ENAC/quadrotor/niquad_wind.xml b/conf/airframes/ENAC/quadrotor/niquad_wind.xml new file mode 100644 index 0000000000..0db55b6395 --- /dev/null +++ b/conf/airframes/ENAC/quadrotor/niquad_wind.xml @@ -0,0 +1,274 @@ + + + + + + * Autopilot: Tawaki + * Actuators: 4 in 4 Holybro BLHELI ESC + * Telemetry: XBee + * GPS: datalink + * RC: Futaba + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + +
+ + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + +
+ + +
+ + + + + + + + + +
+ +
+ + + +
+ +
+ + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ + diff --git a/conf/modules/wind_estimation_quadrotor.xml b/conf/modules/wind_estimation_quadrotor.xml new file mode 100644 index 0000000000..176cb3307b --- /dev/null +++ b/conf/modules/wind_estimation_quadrotor.xml @@ -0,0 +1,50 @@ + + + + + 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. + +
+ + + + + + + + +
+ + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + diff --git a/conf/simulator/jsbsim/aircraft/simple_quad_wind.xml b/conf/simulator/jsbsim/aircraft/simple_quad_wind.xml new file mode 100644 index 0000000000..f299c9631a --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/simple_quad_wind.xml @@ -0,0 +1,314 @@ + + + + + + Gautier Hattenberger + 26-08-2021 + Version 1.0 + + Simple Quadrotor in + configuration without rotor dynamic (front/back motors turning CW, left/right CCW) + + Drag model is linear with speed and correspond to the quadrotor used in + IMAV2021 paper "Estimating wind using a quadrotor" (Hattenberger, Bronz, Condomines) + + + + + 1 + 1 + 1 + 0 + 0 + 0 + 0 + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + + + 0.0068 + 0.0068 + 0.0136 + 0. + 0. + 0. + 0.896 + + 0 + 0 + 0 + + + + + + + -0.25 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0.25 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + 0.25 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + -0.25 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + + fcs/front_motor + fcs/back_motor + fcs/right_motor + fcs/left_motor + + + + + + + fcs/front_motor + 3.6 + 0.224808943 + + + + -0.25 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/back_motor + 3.6 + 0.224808943 + + + + 0.25 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/right_motor + 3.6 + 0.224808943 + + + + 0 + 0.25 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/left_motor + 3.6 + 0.224808943 + + + + 0 + -0.25 + 0 + + + 0 + 0 + -1 + + + + + + + + + fcs/front_motor + 0.1 + 0.738 + + + + -0.25 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/back_motor + 0.1 + 0.738 + + + + 0.25 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/right_motor + 0.1 + 0.738 + + + + 0 + 0.25 + 0 + + + 0 + 0 + 1 + + + + + + + fcs/left_motor + 0.1 + 0.738 + + + + 0 + -0.25 + 0 + + + 0 + 0 + 1 + + + + + + + + + + + + + Drag + + velocities/vtrue-fps + 0.3048 + 0.230292 + 0.224808943 + + + + + + diff --git a/conf/simulator/nps/nps_sensors_params_noisy.h b/conf/simulator/nps/nps_sensors_params_noisy.h new file mode 100644 index 0000000000..121969874b --- /dev/null +++ b/conf/simulator/nps/nps_sensors_params_noisy.h @@ -0,0 +1,194 @@ +/* + * Copyright (C) 2012 Felix Ruess + * + * 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 */ diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index 351179b8fe..1d9fd6e392 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -35,6 +35,7 @@ + diff --git a/sw/airborne/filters/linear_kalman_filter.c b/sw/airborne/filters/linear_kalman_filter.c new file mode 100644 index 0000000000..92b623be81 --- /dev/null +++ b/sw/airborne/filters/linear_kalman_filter.c @@ -0,0 +1,149 @@ +/* + * Copyright (C) 2021 Gautier Hattenberger + * + * 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 + * . + */ +/** + * @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 +} + diff --git a/sw/airborne/filters/linear_kalman_filter.h b/sw/airborne/filters/linear_kalman_filter.h new file mode 100644 index 0000000000..b9c175ecb7 --- /dev/null +++ b/sw/airborne/filters/linear_kalman_filter.h @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2021 Gautier Hattenberger + * + * 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 + * . + */ +/** + * @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 */ diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index ada29adc98..2024f1f2b5 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.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] diff --git a/sw/airborne/modules/air_data/air_data.c b/sw/airborne/modules/air_data/air_data.c index c44f4f040f..9166cba70a 100644 --- a/sw/airborne/modules/air_data/air_data.c +++ b/sw/airborne/modules/air_data/air_data.c @@ -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 diff --git a/sw/airborne/modules/air_data/air_data.h b/sw/airborne/modules/air_data/air_data.h index e8578068f1..f03b0795f8 100644 --- a/sw/airborne/modules/air_data/air_data.h +++ b/sw/airborne/modules/air_data/air_data.h @@ -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 diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h index fa4b643038..aa5e4c7806 100644 --- a/sw/airborne/modules/core/abi_sender_ids.h +++ b/sw/airborne/modules/core/abi_sender_ids.h @@ -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) */ diff --git a/sw/airborne/modules/meteo/wind_estimation_quadrotor.c b/sw/airborne/modules/meteo/wind_estimation_quadrotor.c new file mode 100644 index 0000000000..0cabde372f --- /dev/null +++ b/sw/airborne/modules/meteo/wind_estimation_quadrotor.c @@ -0,0 +1,226 @@ +/* + * Copyright (C) 2021 Gautier Hattenberger + * + * 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 + * . + */ + +/** @file "modules/meteo/wind_estimation_quadrotor.c" + * @author Gautier Hattenberger + * Wind estimation from quadrotor motion + */ + +#include "modules/meteo/wind_estimation_quadrotor.h" +#include "filters/linear_kalman_filter.h" +#include "math/pprz_isa.h" +#include "state.h" +#include "generated/airframe.h" +#include "modules/datalink/downlink.h" +#include "generated/modules.h" +#include "modules/core/abi.h" +#include "modules/air_data/air_data.h" + +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" +#endif + +#define WE_QUAD_STATUS_IDLE 0 +#define WE_QUAD_STATUS_RUN 1 + +#define WE_QUAD_STATE_SIZE 4 +#define WE_QUAD_CMD_SIZE 2 +#define WE_QUAD_MEAS_SIZE 2 + +#define WE_QUAD_VA_X 0 +#define WE_QUAD_W_X 1 +#define WE_QUAD_VA_Y 2 +#define WE_QUAD_W_Y 3 + +#ifndef WE_QUAD_P0_VA +#define WE_QUAD_P0_VA 1.f +#endif + +#ifndef WE_QUAD_P0_W +#define WE_QUAD_P0_W 1.f +#endif + +#ifndef WE_QUAD_Q_VA +#define WE_QUAD_Q_VA .05f +#endif + +#ifndef WE_QUAD_Q_W +#define WE_QUAD_Q_W .001f +#endif + +#ifndef WE_QUAD_R +#define WE_QUAD_R .5f +#endif + +// update wind in state interface directly +#ifndef WE_QUAD_UPDATE_STATE +#define WE_QUAD_UPDATE_STATE TRUE +#endif + +static const float we_dt = WIND_ESTIMATION_QUADROTOR_PERIODIC_PERIOD; + +static void send_wind(struct transport_tx *trans, struct link_device *dev); + +struct wind_estimation_quadrotor { + struct linear_kalman_filter filter; + uint8_t status; +}; + +static struct wind_estimation_quadrotor we_quad; +struct wind_estimation_quadrotor_params we_quad_params; + +static void wind_estimation_quadrotor_reset(void) +{ + we_quad.filter.P[0][0] = WE_QUAD_P0_VA; + we_quad.filter.P[1][1] = WE_QUAD_P0_W; + we_quad.filter.P[2][2] = WE_QUAD_P0_VA; + we_quad.filter.P[3][3] = WE_QUAD_P0_W; + float_vect_zero(we_quad.filter.X, WE_QUAD_STATE_SIZE); +} + +void wind_estimation_quadrotor_init(void) +{ + we_quad.status = WE_QUAD_STATUS_IDLE; + + // init filter and model + linear_kalman_filter_init(&we_quad.filter, WE_QUAD_STATE_SIZE, WE_QUAD_CMD_SIZE, WE_QUAD_MEAS_SIZE); + we_quad.filter.A[0][0] = 1.f - WE_QUAD_DRAG * we_dt / WE_QUAD_MASS; + we_quad.filter.A[1][1] = 1.f; + we_quad.filter.A[2][2] = 1.f - WE_QUAD_DRAG * we_dt / WE_QUAD_MASS; + we_quad.filter.A[3][3] = 1.f; + we_quad.filter.B[0][0] = we_dt / WE_QUAD_MASS; + we_quad.filter.B[2][1] = we_dt / WE_QUAD_MASS; + we_quad.filter.C[0][0] = 1.f; + we_quad.filter.C[0][1] = 1.f; + we_quad.filter.C[1][2] = 1.f; + we_quad.filter.C[1][3] = 1.f; + we_quad.filter.Q[0][0] = WE_QUAD_Q_VA; + we_quad.filter.Q[1][1] = WE_QUAD_Q_W; + we_quad.filter.Q[2][2] = WE_QUAD_Q_VA; + we_quad.filter.Q[3][3] = WE_QUAD_Q_W; + we_quad.filter.R[0][0] = WE_QUAD_R; + we_quad.filter.R[1][1] = WE_QUAD_R; + // reset covariance and state + wind_estimation_quadrotor_reset(); + // external params + we_quad_params.Q_va = WE_QUAD_Q_VA; + we_quad_params.Q_w = WE_QUAD_Q_W; + we_quad_params.R = WE_QUAD_R; + +#if PERIODIC_TELEMETRY + // register for periodic telemetry + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WIND_INFO_RET, send_wind); +#endif +} + +void wind_estimation_quadrotor_periodic(void) +{ + // compute command from state interface + float U[WE_QUAD_CMD_SIZE]; + struct FloatRMat rot = *stateGetNedToBodyRMat_f(); + float coef = Max(MAT33_ELMT(rot, 2, 2), 0.5); // limit to 1/2 + float Tnorm = WE_QUAD_MASS * PPRZ_ISA_GRAVITY / coef; + struct FloatVect3 Tbody = { 0.f, 0.f, -Tnorm }; + struct FloatVect3 Tned; + float_rmat_transp_vmult(&Tned, &rot, &Tbody); + U[0] = Tned.x; + U[1] = Tned.y; + + linear_kalman_filter_predict(&we_quad.filter, U); + + // compute correction from measure + float Y[WE_QUAD_MEAS_SIZE]; + Y[0] = stateGetSpeedNed_f()->x; // north ground speed + Y[1] = stateGetSpeedNed_f()->y; // east ground speed + + linear_kalman_filter_update(&we_quad.filter,Y); + + // send airspeed as ABI message + struct FloatVect2 tas = { we_quad.filter.X[WE_QUAD_VA_X], we_quad.filter.X[WE_QUAD_VA_Y] }; + float eas = eas_from_tas(float_vect2_norm(&tas)); + AbiSendMsgAIRSPEED(AIRSPEED_WE_QUAD_ID, eas); + // update wind in state interface only if enabled +#if WE_QUAD_UPDATE_STATE + struct FloatVect2 wind_ne = { + we_quad.filter.X[WE_QUAD_W_X], + we_quad.filter.X[WE_QUAD_W_Y] + }; + stateSetHorizontalWindspeed_f(&wind_ne); +#endif +} + +void wind_estimation_quadrotor_stop(void) +{ + we_quad.status = WE_QUAD_STATUS_IDLE; +#if WE_QUAD_UPDATE_STATE + struct FloatVect2 zero_wind_ne = { 0.f, 0.f }; + stateSetHorizontalWindspeed_f(&zero_wind_ne); +#endif +} + +void wind_estimation_quadrotor_start(void) +{ + wind_estimation_quadrotor_reset(); + we_quad.status = WE_QUAD_STATUS_RUN; +} + +float wind_estimation_quadrotor_SetQva(float Q_va) +{ + we_quad_params.Q_va = Q_va; + we_quad.filter.Q[0][0] = Q_va; + we_quad.filter.Q[2][2] = Q_va; + return Q_va; +} + +float wind_estimation_quadrotor_SetQw(float Q_w) +{ + we_quad_params.Q_w = Q_w; + we_quad.filter.Q[1][1] = Q_w; + we_quad.filter.Q[3][3] = Q_w; + return Q_w; +} + +float wind_estimation_quadrotor_SetR(float R) +{ + we_quad_params.R = R; + we_quad.filter.R[0][0] = R; + we_quad.filter.R[1][1] = R; + return R; +} + +void wind_estimation_quadrotor_report(void) +{ + DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, WE_QUAD_STATE_SIZE, we_quad.filter.X); +} + +static void send_wind(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t flags = 5; // send horizontal wind and airspeed + float upwind = 0.f; + struct FloatVect2 as = { we_quad.filter.X[WE_QUAD_VA_X], we_quad.filter.X[WE_QUAD_VA_Y] }; + float airspeed = float_vect2_norm(&as); + pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, + &flags, + &we_quad.filter.X[WE_QUAD_W_Y], // east + &we_quad.filter.X[WE_QUAD_W_X], // north + &upwind, + &airspeed); +} + diff --git a/sw/airborne/modules/meteo/wind_estimation_quadrotor.h b/sw/airborne/modules/meteo/wind_estimation_quadrotor.h new file mode 100644 index 0000000000..183fd80a66 --- /dev/null +++ b/sw/airborne/modules/meteo/wind_estimation_quadrotor.h @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2021 Gautier Hattenberger + * + * 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 + * . + */ + +/** @file "modules/meteo/wind_estimation_quadrotor.h" + * @author Gautier Hattenberger + * 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 diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp index 48d7439eeb..9c7ca40a73 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.cpp +++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp @@ -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;