diff --git a/conf/airframes/examples/bebop2_opticflow.xml b/conf/airframes/examples/bebop2_opticflow.xml index 3face7cae2..d3d9643485 100644 --- a/conf/airframes/examples/bebop2_opticflow.xml +++ b/conf/airframes/examples/bebop2_opticflow.xml @@ -3,8 +3,16 @@ - - + + + + + @@ -14,30 +22,104 @@ + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -70,6 +152,19 @@ + +
+ + + + + + +
+
@@ -84,30 +179,36 @@ + + + + + + +
+ - - +--> + - - + + - +-->
@@ -119,6 +220,14 @@
+
+ + + + +
+ +
@@ -141,9 +250,18 @@ - + + + + + + + + @@ -152,7 +270,16 @@ - + + + + + @@ -182,13 +309,13 @@ - +
- - + +
@@ -197,20 +324,43 @@
+ + + + + + + + - + +
+
+ +
+
diff --git a/conf/airframes/tudelft/bebop2_opticflow.xml b/conf/airframes/tudelft/tudelft_bebop2_opticflow_sim.xml similarity index 74% rename from conf/airframes/tudelft/bebop2_opticflow.xml rename to conf/airframes/tudelft/tudelft_bebop2_opticflow_sim.xml index 851d86c05f..c232c30b40 100644 --- a/conf/airframes/tudelft/bebop2_opticflow.xml +++ b/conf/airframes/tudelft/tudelft_bebop2_opticflow_sim.xml @@ -1,13 +1,19 @@ - ? - - - + + + + + + + + + + @@ -15,46 +21,71 @@ + + + + + + + + - - - - + + + + + + + + + + + - - + + + + + + + - + + @@ -103,28 +134,28 @@
- - + + - + +--> + - + - +-->
@@ -136,6 +167,14 @@
+
+ + + + +
+ +
@@ -154,12 +193,14 @@
+ +
- - - - - + + + + + @@ -169,12 +210,17 @@ - + + + + + + - - - + + + @@ -208,17 +254,21 @@
-
- - -
-
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/cv_textons.xml b/conf/modules/cv_textons.xml index de661160d3..5a66412d3a 100644 --- a/conf/modules/cv_textons.xml +++ b/conf/modules/cv_textons.xml @@ -5,16 +5,21 @@ Represent the appearance (texture, color) of an image by means of a texton histogram.
+ + + - + + + + - - +
@@ -22,14 +27,16 @@ + + - + - + @@ -41,7 +48,7 @@ - + diff --git a/conf/modules/ins_flow.xml b/conf/modules/ins_flow.xml new file mode 100644 index 0000000000..b1e967976d --- /dev/null +++ b/conf/modules/ins_flow.xml @@ -0,0 +1,47 @@ + + + + + + simple INS and AHRS using flow + + + + + + + + + + + + + + + + @imu + ahrs,ins + +
+ +
+ + + + + + + + + + + + + + + $(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_flow.h\" + $(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_icq + + + + diff --git a/conf/modules/optical_flow_landing.xml b/conf/modules/optical_flow_landing.xml index 409cd23eb9..1fc80a203b 100644 --- a/conf/modules/optical_flow_landing.xml +++ b/conf/modules/optical_flow_landing.xml @@ -11,17 +11,27 @@
- - - + + + - + - - + + + + + + + + + + + +
@@ -36,14 +46,32 @@ - + + + + + + + + + + + + + + + + + + + @@ -52,8 +80,10 @@ - + + + diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml index be1578189c..bf8faa94b0 100644 --- a/conf/telemetry/highspeed_rotorcraft.xml +++ b/conf/telemetry/highspeed_rotorcraft.xml @@ -45,6 +45,7 @@ + diff --git a/conf/userconf/tudelft/guido_conf.xml b/conf/userconf/tudelft/guido_conf.xml index f96decd625..73624c7cc6 100644 --- a/conf/userconf/tudelft/guido_conf.xml +++ b/conf/userconf/tudelft/guido_conf.xml @@ -7,23 +7,34 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_guido_optitrack.xml" settings="settings/rotorcraft_basic.xml" - gui_color="#fffff996b847" settings_modules="modules/ahrs_int_cmpl_quat.xml modules/cv_opticflow.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_landing.xml modules/stabilization_indi_simple.xml" + gui_color="#fffff996b847" /> + - - - - + + + + + + + + - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - + + +
-
- + + + + + - - + + + + - - - + + + + + + + - - - - - - - + + + + + + + + + + + + + + + + + + + + + + - - + + + - + @@ -90,156 +91,36 @@ - - - + + + - - - - - + + - - - - - - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - + + - - - - - - - - - - - - - - -
diff --git a/data/pictures/gcs_icons/learn.png b/data/pictures/gcs_icons/learn.png new file mode 100644 index 0000000000..3fe4a09647 Binary files /dev/null and b/data/pictures/gcs_icons/learn.png differ diff --git a/sw/airborne/math/RANSAC.c b/sw/airborne/math/RANSAC.c index a90a65ace5..9c1f00b11b 100644 --- a/sw/airborne/math/RANSAC.c +++ b/sw/airborne/math/RANSAC.c @@ -55,17 +55,19 @@ * */ void RANSAC_linear_model(int n_samples, int n_iterations, float error_threshold, float *targets, int D, - float (*samples)[D], uint16_t count, float *params, float *fit_error __attribute__((unused))) + float (*samples)[D], uint16_t count, bool use_bias, float *params, float *fit_error __attribute__((unused))) { - uint8_t D_1 = D + 1; + uint8_t D_1 = D; + if (use_bias) { + D_1++; + } float err; float errors[n_iterations]; int indices_subset[n_samples]; float subset_targets[n_samples]; float subset_samples[n_samples][D]; float subset_params[n_iterations][D_1]; - bool use_bias = true; // ensure that n_samples is high enough to ensure a result for a single fit: n_samples = (n_samples < D_1) ? D_1 : n_samples; @@ -88,12 +90,7 @@ void RANSAC_linear_model(int n_samples, int n_iterations, float error_threshold, // fit a linear model on the small system: fit_linear_model(subset_targets, D, subset_samples, n_samples, use_bias, subset_params[i], &err); - printf("params normal: %f, %f\n", subset_params[i][0], subset_params[i][1]); - float priors[2]; - priors[0] = 1.0f; - priors[1] = 10.0f; - fit_linear_model_prior(subset_targets, D, subset_samples, n_samples, use_bias, priors, subset_params[i], &err); - printf("params prior: %f, %f\n", subset_params[i][0], subset_params[i][1]); + // determine the error on the whole set: float err_sum = 0.0f; diff --git a/sw/airborne/math/RANSAC.h b/sw/airborne/math/RANSAC.h index c5844b0a28..7095c0a270 100644 --- a/sw/airborne/math/RANSAC.h +++ b/sw/airborne/math/RANSAC.h @@ -50,11 +50,12 @@ extern "C" { * @param[in] samples The samples / feature vectors * @param[in] D The dimensionality of the samples * @param[in] count The number of samples + * @param[in] use_bias Whether the RANSAC procedure should add a bias. If 0 it does not. * @param[out] parameters* Parameters of the linear fit * @param[out] fit_error* Total error of the fit */ void RANSAC_linear_model(int n_samples, int n_iterations, float error_threshold, float *targets, int D, - float (*samples)[D], uint16_t count, float *params, float *fit_error); + float (*samples)[D], uint16_t count, bool use_bias, float *params, float *fit_error); /** Get indices without replacement. * diff --git a/sw/airborne/math/pprz_algebra_float.c b/sw/airborne/math/pprz_algebra_float.c index 9352b95204..ef0503f7ea 100644 --- a/sw/airborne/math/pprz_algebra_float.c +++ b/sw/airborne/math/pprz_algebra_float.c @@ -1064,4 +1064,3 @@ void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des) { vect2->y *= scale; } } - diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 7dac5d2da0..7c68805f52 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -881,6 +881,31 @@ static inline void float_mat_diagonal_scal(float **o, float v, int n) } } + +/** Divide a matrix by a scalar */ +static inline void float_mat_div_scalar(float **o, float **a, float scalar, int m, int n) +{ + int i, j; + for (i = 0; i < m; i++) { + for (j = 0; j < n; j++) { + o[i][j] = a[i][j] / scalar; + } + } +} + +/** Multiply a matrix by a scalar */ +static inline void float_mat_mul_scalar(float **o, float **a, float scalar, int m, int n) +{ + int i, j; + for (i = 0; i < m; i++) { + for (j = 0; j < n; j++) { + o[i][j] = a[i][j] * scalar; + } + } +} + + + extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]); extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in); extern bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4]); diff --git a/sw/airborne/modules/ahrs/ahrs.h b/sw/airborne/modules/ahrs/ahrs.h index 676d9ec027..e85b143880 100644 --- a/sw/airborne/modules/ahrs/ahrs.h +++ b/sw/airborne/modules/ahrs/ahrs.h @@ -43,6 +43,7 @@ #define AHRS_COMP_ID_VECTORNAV 11 #define AHRS_COMP_ID_EKF2 12 #define AHRS_COMP_ID_MADGWICK 13 +#define AHRS_COMP_ID_FLOW 14 /* include actual (primary) implementation header */ #ifdef AHRS_TYPE_H diff --git a/sw/airborne/modules/computer_vision/opticflow/linear_flow_fit.c b/sw/airborne/modules/computer_vision/opticflow/linear_flow_fit.c index 44e8782b20..ba4e4afd03 100644 --- a/sw/airborne/modules/computer_vision/opticflow/linear_flow_fit.c +++ b/sw/airborne/modules/computer_vision/opticflow/linear_flow_fit.c @@ -42,12 +42,16 @@ #include "math/pprz_algebra_float.h" #include "math/pprz_matrix_decomp_float.h" #include "math/pprz_simple_matrix.h" +#include "math/RANSAC.h" +#include "std.h" // Is this still necessary? #define MAX_COUNT_PT 50 #define MIN_SAMPLES_FIT 3 +#define N_PAR_TR_FIT 6 + /** * Analyze a linear flow field, retrieving information such as divergence, surface roughness, focus of expansion, etc. * @param[out] outcome If 0, there were too few vectors for a fit. If 1, the fit was successful. @@ -60,7 +64,8 @@ * @param[in] im_height Image height in pixels * @param[out] info Contains all info extracted from the linear flow fit. */ -bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info) +bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, + int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info) { // Are there enough flow vectors to perform a fit? if (count < MIN_SAMPLES_FIT) { @@ -70,7 +75,8 @@ bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_th // fit linear flow field: float parameters_u[3], parameters_v[3], min_error_u, min_error_v; - fit_linear_flow_field(vectors, count, n_iterations, error_threshold, n_samples, parameters_u, parameters_v, &info->fit_error, &min_error_u, &min_error_v, &info->n_inliers_u, &info->n_inliers_v); + fit_linear_flow_field(vectors, count, n_iterations, error_threshold, n_samples, parameters_u, parameters_v, + &info->fit_error, &min_error_u, &min_error_v, &info->n_inliers_u, &info->n_inliers_v); // extract information from the parameters: extract_information_from_parameters(parameters_u, parameters_v, im_width, im_height, info); @@ -100,7 +106,9 @@ bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_th * @param[out] n_inliers_u* Number of inliers in the horizontal flow fit. * @param[out] n_inliers_v* Number of inliers in the vertical flow fit. */ -void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u, int *n_inliers_v) +void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, + float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u, + int *n_inliers_v) { // We will solve systems of the form A x = b, @@ -310,7 +318,8 @@ void fit_linear_flow_field(struct flow_t *vectors, int count, float error_thresh * @param[out] info Contains all info extracted from the linear flow fit */ -void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height, struct linear_flow_fit_info *info) +void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height, + struct linear_flow_fit_info *info) { // This method assumes a linear flow field in x- and y- direction according to the formulas: // u = parameters_u[0] * x + parameters_u[1] * y + parameters_u[2] @@ -323,8 +332,10 @@ void extract_information_from_parameters(float *parameters_u, float *parameters_ // translation orthogonal to the camera axis: // flow in the center of the image: - info->relative_velocity_x = -(parameters_u[2] + (im_width / 2.0f) * parameters_u[0] + (im_height / 2.0f) * parameters_u[1]); - info->relative_velocity_y = -(parameters_v[2] + (im_width / 2.0f) * parameters_v[0] + (im_height / 2.0f) * parameters_v[1]); + info->relative_velocity_x = -(parameters_u[2] + (im_width / 2.0f) * parameters_u[0] + + (im_height / 2.0f) * parameters_u[1]); + info->relative_velocity_y = -(parameters_v[2] + (im_width / 2.0f) * parameters_v[0] + + (im_height / 2.0f) * parameters_v[1]); float arv_x = fabsf(info->relative_velocity_x); float arv_y = fabsf(info->relative_velocity_y); @@ -372,3 +383,109 @@ void extract_information_from_parameters(float *parameters_u, float *parameters_ } else { info->focus_of_expansion_y = 0.0f; } } +/** + * Analyze a flow field, retrieving information on relative velocities and rotations, etc. + * @param[out] outcome If 0, there were too few vectors for a fit. If 1, the fit was successful. + * @param[in] vectors The optical flow vectors + * @param[in] count The number of optical flow vectors + * @param[in] error_threshold Error used to determine inliers / outliers. + * @param[in] n_iterations Number of RANSAC iterations. + * @param[in] n_samples Number of samples used for a single fit (min. 5). + * @param[in] im_width Image width in pixels + * @param[in] im_height Image height in pixels + * @param[in] focal_length Focal length in pixels + * @param[out] info Contains all info extracted from the linear flow fit. + */ +bool analyze_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, + int im_width, int im_height, float focal_length, struct linear_flow_fit_info *info) +{ + // Are there enough flow vectors to perform a fit? + if (count < N_PAR_TR_FIT) { + // return that no fit was made: + return false; + } + return true; + + /* + // fit linear flow field: + //float parameters_u[N_PAR_TR_FIT], parameters_v[N_PAR_TR_FIT], min_error_u, min_error_v; + float parameters_comb[N_PAR_TR_FIT], min_error_comb; + + // normalize the flow vectors. Is supposed to be better for the fit and the parameters will be easier to interpret: + struct flow_t *normalized_vectors = (struct flow_t *) malloc(count *sizeof(struct flow_t)); + + + //float targets_x[count]; + //float targets_y[count]; + + // separate horizontal / vertical fit: + //float D_x[count][N_PAR_TR_FIT-1]; // bias is added in the RANSAC routine + //float D_y[count][N_PAR_TR_FIT-1]; + + // combined fit: + int total_count = 2*count; + float targets[total_count]; + float D_comb[total_count][N_PAR_TR_FIT]; + int index; + for (int i = 0; i < count; i++) { + + // normalize vectors: + normalized_vectors[i].pos.x = (vectors[i].pos.x - (im_width / 2.0f)) / focal_length; + normalized_vectors[i].pos.y = (vectors[i].pos.y - (im_height / 2.0f)) / focal_length; + normalized_vectors[i].flow_x = vectors[i].flow_x / focal_length; + normalized_vectors[i].flow_y = vectors[i].flow_y / focal_length; + + + // Combine vertical and horizontal flow in one system: + // As in "Estimation of Visual Motion Parameters Used for Bio-inspired Navigation", by Alkowatly et al. + index = i*2; + targets[index] = normalized_vectors[i].flow_x; + D_comb[index][0] = 1.0f; + D_comb[index][1] = normalized_vectors[i].pos.x; + D_comb[index][2] = normalized_vectors[i].pos.y; + D_comb[index][3] = normalized_vectors[i].pos.x * normalized_vectors[i].pos.y; + D_comb[index][4] = normalized_vectors[i].pos.x * normalized_vectors[i].pos.x; + D_comb[index][5] = 0.0f; + + index++; + targets[index] = normalized_vectors[i].flow_y; + D_comb[index][0] = 0.0f; + D_comb[index][1] = normalized_vectors[i].pos.y; + D_comb[index][1] = -normalized_vectors[i].pos.x; + D_comb[index][2] = normalized_vectors[i].pos.y * normalized_vectors[i].pos.y; + D_comb[index][3] = normalized_vectors[i].pos.x * normalized_vectors[i].pos.y; + D_comb[index][4] = 1.0f; + + } + + // Perform RANSAC on the horizontal flow: + // RANSAC_linear_model(n_samples, n_iterations, error_threshold, targets_x, N_PAR_TR_FIT, D_x, count, parameters_u, &min_error_u); + + // Perform RANSAC on the combined system: + bool use_bias = false; + RANSAC_linear_model(n_samples, n_iterations, error_threshold, targets, N_PAR_TR_FIT+1, D_comb, total_count, parameters_comb, &min_error_comb, use_bias); + + // extract information from the parameters: + info->rotation_X = parameters_comb[3]; + info->rotation_Y = -parameters_comb[4]; + info->rotation_Z = parameters_comb[2]; + info->divergence = parameters_comb[1]; + info->relative_velocity_x = -parameters_comb[0] - info->rotation_Y; + info->relative_velocity_y = -parameters_comb[5] + info->rotation_X; + if(fabs(parameters_comb[1]) > 1E-3) { + info->focus_of_expansion_x = info->relative_velocity_x / parameters_comb[1]; + info->focus_of_expansion_y = info->relative_velocity_y / parameters_comb[1]; + } + else { + // FoE in the center of the image: + info->focus_of_expansion_x = 0.0f; + info->focus_of_expansion_y = 0.0f; + } + + // free up allocated memory: + free(normalized_vectors); + + // return successful fit: + return true; + */ +} diff --git a/sw/airborne/modules/computer_vision/opticflow/linear_flow_fit.h b/sw/airborne/modules/computer_vision/opticflow/linear_flow_fit.h index d680b3d4ef..2bee75f042 100644 --- a/sw/airborne/modules/computer_vision/opticflow/linear_flow_fit.h +++ b/sw/airborne/modules/computer_vision/opticflow/linear_flow_fit.h @@ -51,16 +51,27 @@ struct linear_flow_fit_info { float fit_error; ///< Error of the fit (same as surface roughness) int n_inliers_u; ///< Number of inliers in the horizontal flow fit int n_inliers_v; ///< Number of inliers in the vertical flow fit + float rotation_X; ///< Rotation around the X axis + float rotation_Y; ///< Rotation around the Y axis + float rotation_Z; ///< Rotation around the Z axis }; // This is the function called externally, passing the vector of optical flow vectors and information on the number of vectors and image size: -bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info); +bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, + int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info); // Fits the linear flow field with RANSAC: -void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u, int *n_inliers_v); +void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, + float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u, + int *n_inliers_v); // Extracts relevant information from the fit parameters: -void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height, struct linear_flow_fit_info *info); +void extract_information_from_parameters(float *parameters_u, float *parameters_v, int im_width, int im_height, + struct linear_flow_fit_info *info); + +// Analyze the optic flow field, also determining the rotations: +bool analyze_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, + int im_width, int im_height, float focal_length, struct linear_flow_fit_info *info); #endif diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index 4ff117de0a..78cd27f6cc 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -63,7 +63,7 @@ uint16_t n_agents[2] = {25, 25}; #define SIZE_DIV 1 // LINEAR_FIT makes a linear optical flow field fit and extracts a lot of information: // relative velocities in x, y, z (divergence / time to contact), the slope of the surface, and the surface roughness. -#define LINEAR_FIT 1 +#define LINEAR_FIT 0 #ifndef OPTICFLOW_CORNER_METHOD #define OPTICFLOW_CORNER_METHOD ACT_FAST @@ -477,7 +477,8 @@ void opticflow_calc_init(struct opticflow_t opticflow[]) opticflow[1].id = 1; struct FloatEulers euler_cam2 = {OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2, OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2, - OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2}; + OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2 + }; float_rmat_of_eulers(&body_to_cam[1], &euler_cam2); #endif } @@ -682,6 +683,14 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, result->divergence = fit_info.divergence; result->surface_roughness = fit_info.surface_roughness; + + // Flow fit with rotations: + error_threshold = 10.0f; + n_iterations_RANSAC = 20; + n_samples_RANSAC = 5; + success_fit = analyze_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC, + n_samples_RANSAC, img->w, img->h, OPTICFLOW_CAMERA.camera_intrinsics.focal_x, &fit_info); + } else { result->divergence = 0.0f; result->surface_roughness = 0.0f; @@ -728,14 +737,15 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, } else { - // determine the roll, pitch, yaw differencces between the images. + // determine the roll, pitch, yaw differences between the images. float phi_diff = opticflow->img_gray.eulers.phi - opticflow->prev_img_gray.eulers.phi; float theta_diff = opticflow->img_gray.eulers.theta - opticflow->prev_img_gray.eulers.theta; float psi_diff = opticflow->img_gray.eulers.psi - opticflow->prev_img_gray.eulers.psi; if (strcmp(opticflow->camera->dev_name, bottom_camera.dev_name) == 0) { // bottom cam: just subtract a scaled version of the roll and pitch difference from the global flow vector: - diff_flow_x = phi_diff * opticflow->camera->camera_intrinsics.focal_x; // phi_diff works better than (cam_state->rates.p) + diff_flow_x = phi_diff * + opticflow->camera->camera_intrinsics.focal_x; // phi_diff works better than (cam_state->rates.p) diff_flow_y = theta_diff * opticflow->camera->camera_intrinsics.focal_y; result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor * opticflow->derotation_correction_factor_x; @@ -1113,7 +1123,8 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, result->tracked_cnt = getAmountPeaks(edge_hist_x, 500, img->w); result->divergence = -1.0 * (float)edgeflow.div_x / RES; // Also multiply the divergence with -1.0 to make it on par with the LK algorithm of - result->div_size = result->divergence; // Fill the div_size with the divergence to atleast get some divergenge measurement when switching from LK to EF + result->div_size = + result->divergence; // Fill the div_size with the divergence to atleast get some divergenge measurement when switching from LK to EF result->camera_id = opticflow->id; result->surface_roughness = 0.0f; //......................Calculating VELOCITY ..................... // diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h index f8118c1c3b..74365a745a 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h @@ -79,6 +79,8 @@ struct opticflow_t { int actfast_min_gradient; ///< Threshold that decides when there is sufficient texture for edge following int actfast_gradient_method; ///< Whether to use a simple or Sobel filter + uint16_t fps; + const struct video_config_t *camera; uint8_t id; }; @@ -87,15 +89,16 @@ struct opticflow_t { extern void opticflow_calc_init(struct opticflow_t opticflow[]); extern bool opticflow_calc_frame(struct opticflow_t *opticflow, struct image_t *img, - struct opticflow_result_t *result); + struct opticflow_result_t *result); extern bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, - struct opticflow_result_t *result); + struct opticflow_result_t *result); extern bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, - struct opticflow_result_t *result); + struct opticflow_result_t *result); -extern void kalman_filter_opticflow_velocity(float *velocity_x, float *velocity_y, float *acceleration_measurement, float fps, - float *measurement_noise, float process_noise, bool reinitialize_kalman); +extern void kalman_filter_opticflow_velocity(float *velocity_x, float *velocity_y, float *acceleration_measurement, + float fps, + float *measurement_noise, float process_noise, bool reinitialize_kalman); #endif /* OPTICFLOW_CALCULATOR_H */ diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index fd4ebb7fb0..12eec0b9c0 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -40,6 +40,8 @@ #include "cv.h" +uint16_t fps_OF; + /* ABI messages sender ID */ #ifndef OPTICFLOW_AGL_ID #define OPTICFLOW_AGL_ID ABI_BROADCAST ///< Default sonar/agl to use in opticflow visual_estimator @@ -70,7 +72,8 @@ static bool opticflow_got_result[ACTIVE_CAMERAS]; ///< When we have an opt static pthread_mutex_t opticflow_mutex; ///< Mutex lock fo thread safety /* Static functions */ -struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id); ///< The main optical flow calculation thread +struct image_t *opticflow_module_calc(struct image_t *img, + uint8_t camera_id); ///< The main optical flow calculation thread #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" @@ -149,7 +152,7 @@ void opticflow_module_run(void) opticflow_result[idx_camera].noise_measurement, opticflow_result[idx_camera].noise_measurement, -1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity. - ); + ); } opticflow_got_result[idx_camera] = false; } @@ -174,8 +177,9 @@ struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id) img->eulers = pose.eulers; // Do the optical flow calculation - static struct opticflow_result_t temp_result[ACTIVE_CAMERAS]; // static so that the number of corners is kept between frames - if(opticflow_calc_frame(&opticflow[camera_id], img, &temp_result[camera_id])){ + static struct opticflow_result_t + temp_result[ACTIVE_CAMERAS]; // static so that the number of corners is kept between frames + if (opticflow_calc_frame(&opticflow[camera_id], img, &temp_result[camera_id])) { // Copy the result if finished pthread_mutex_lock(&opticflow_mutex); opticflow_result[camera_id] = temp_result[camera_id]; diff --git a/sw/airborne/modules/computer_vision/textons.c b/sw/airborne/modules/computer_vision/textons.c index 186bfe4bb5..3cc0957913 100644 --- a/sw/airborne/modules/computer_vision/textons.c +++ b/sw/airborne/modules/computer_vision/textons.c @@ -33,20 +33,38 @@ #include #include "modules/computer_vision/cv.h" #include "modules/computer_vision/textons.h" +#include "mcu_periph/sys_time.h" float ** **dictionary; uint32_t learned_samples = 0; uint8_t dictionary_initialized = 0; float *texton_distribution; +#define MAX_N_TEXTONS 255 + // initial settings: +#ifndef TEXTONS_RUN +#define TEXTONS_RUN 1 +#endif +PRINT_CONFIG_VAR(TEXTONS_RUN) + +#ifndef TEXTONS_FPS +#define TEXTONS_FPS 30 +#endif +PRINT_CONFIG_VAR(TEXTONS_FPS) + #ifndef TEXTONS_LOAD_DICTIONARY #define TEXTONS_LOAD_DICTIONARY 1 #endif PRINT_CONFIG_VAR(TEXTONS_LOAD_DICTIONARY) +#ifndef TEXTONS_REINITIALIZE_DICTIONARY +#define TEXTONS_REINITIALIZE_DICTIONARY 0 +#endif +PRINT_CONFIG_VAR(TEXTONS_REINITIALIZE_DICTIONARY) + #ifndef TEXTONS_ALPHA -#define TEXTONS_ALPHA 10 +#define TEXTONS_ALPHA 0 #endif PRINT_CONFIG_VAR(TEXTONS_ALPHA) @@ -56,7 +74,7 @@ PRINT_CONFIG_VAR(TEXTONS_ALPHA) PRINT_CONFIG_VAR(TEXTONS_N_TEXTONS) #ifndef TEXTONS_N_SAMPLES -#define TEXTONS_N_SAMPLES 100 +#define TEXTONS_N_SAMPLES 250 #endif PRINT_CONFIG_VAR(TEXTONS_N_SAMPLES) @@ -66,7 +84,7 @@ PRINT_CONFIG_VAR(TEXTONS_N_SAMPLES) PRINT_CONFIG_VAR(TEXTONS_PATCH_SIZE) #ifndef TEXTONS_N_LEARNING_SAMPLES -#define TEXTONS_N_LEARNING_SAMPLES 10000 +#define TEXTONS_N_LEARNING_SAMPLES 5000 #endif PRINT_CONFIG_VAR(TEXTONS_N_LEARNING_SAMPLES) @@ -90,8 +108,15 @@ PRINT_CONFIG_VAR(TEXTONS_BORDER_HEIGHT) #endif PRINT_CONFIG_VAR(TEXTONS_DICTIONARY_NUMBER) +#ifndef TEXTONS_DICTIONARY_PATH +#define TEXTONS_DICTIONARY_PATH /data/ftp/internal_000 +#endif +struct video_listener *listener = NULL; + +uint8_t running = TEXTONS_RUN; uint8_t load_dictionary = TEXTONS_LOAD_DICTIONARY; +uint8_t reinitialize_dictionary = TEXTONS_REINITIALIZE_DICTIONARY; uint8_t alpha_uint = TEXTONS_ALPHA; uint8_t n_textons = TEXTONS_N_TEXTONS; uint8_t patch_size = TEXTONS_PATCH_SIZE; @@ -108,18 +133,17 @@ float alpha = 0.0; // File pointer for saving the dictionary static FILE *dictionary_logger = NULL; -#ifndef DICTIONARY_PATH -#define DICTIONARY_PATH /data/video/ -#endif /** * Main texton processing function that first either loads or learns a dictionary and then extracts the texton histogram. * @param[out] *img The output image * @param[in] *img The input image (YUV422) */ -struct image_t *texton_func(struct image_t *img); -struct image_t *texton_func(struct image_t *img) +struct image_t *texton_func(struct image_t *img, uint8_t p); +struct image_t *texton_func(struct image_t *img, uint8_t p) { + // whether to execute the function: + if (!running) { return img; } if (img->buf_size == 0) { return img; } @@ -129,9 +153,24 @@ struct image_t *texton_func(struct image_t *img) // if patch size odd, correct: if (patch_size % 2 == 1) { patch_size++; } + // check whether we have to reinitialize the dictionary: + if (reinitialize_dictionary) { + // set all vars to trigger a reinitialization and learning phase of the dictionary: + dictionary_ready = 0; + dictionary_initialized = 0; + load_dictionary = 0; + learned_samples = 0; + alpha_uint = 10; + // reset reinitialize_dictionary + reinitialize_dictionary = 0; + } + // if dictionary not initialized: if (dictionary_ready == 0) { if (load_dictionary == 0) { + + printf("Learned samples: %d / %d\n", learned_samples, n_learning_samples); + // Train the dictionary: DictionaryTrainingYUV(frame, img->w, img->h); @@ -143,14 +182,35 @@ struct image_t *texton_func(struct image_t *img) dictionary_ready = 1; // lower learning rate alpha = 0.0; + printf("Enough learning!\n"); + alpha_uint = 0; + // set learned samples back to 0 + learned_samples = 0; } } else { // Load the dictionary: load_texton_dictionary(); } } else { - // Extract distributions - DistributionExtraction(frame, img->w, img->h); + if (alpha_uint > 0) { + + // printf("Learning, frame time = %d\n", img->ts.tv_sec * 1000 + img->ts.tv_usec / 1000); + + DictionaryTrainingYUV(frame, img->w, img->h); + + if (learned_samples >= n_learning_samples) { + // Save the dictionary: + save_texton_dictionary(); + // reset learned_samples: + learned_samples = 0; + } + } else { + // Extract distributions + DistributionExtraction(frame, img->w, img->h); + } + + // printf("N textons = %d\n", n_samples_image); + // printf("Entropy texton distribution = %f\n", get_entropy(texton_distribution, n_textons)); } return img; // Colorfilter did not make a new image @@ -320,8 +380,6 @@ void DistributionExtraction(uint8_t *frame, uint16_t width, uint16_t height) // EXECUTION // ************************ - printf("Execute!\n"); - // Allocate memory for texton distances and image patch: float *texton_distances, * **patch; texton_distances = (float *)calloc(n_textons, sizeof(float)); @@ -411,9 +469,10 @@ void DistributionExtraction(uint8_t *frame, uint16_t width, uint16_t height) } // Normalize distribution: - for (i = 0; i < n_textons; i++) { - texton_distribution[i] = texton_distribution[i] / (float) n_extracted_textons; - // printf("textons[%d] = %f\n", i, texton_distribution[i]); + if (n_extracted_textons > 0) { // should always be the case + for (i = 0; i < n_textons; i++) { + texton_distribution[i] = texton_distribution[i] / (float) n_extracted_textons; + } } // printf("\n"); @@ -444,11 +503,12 @@ void save_texton_dictionary(void) char filename[512]; // Check for available files - sprintf(filename, "%s/Dictionary_%05d.dat", STRINGIFY(DICTIONARY_PATH), dictionary_number); + sprintf(filename, "%s/Dictionary_%05d.dat", STRINGIFY(TEXTONS_DICTIONARY_PATH), dictionary_number); dictionary_logger = fopen(filename, "w"); if (dictionary_logger == NULL) { + printf("Filename: %s\n", filename); perror("Error while opening the file.\n"); } else { // (over-)write dictionary @@ -471,7 +531,7 @@ void save_texton_dictionary(void) void load_texton_dictionary(void) { char filename[512]; - sprintf(filename, "%s/Dictionary_%05d.dat", STRINGIFY(DICTIONARY_PATH), dictionary_number); + sprintf(filename, "%s/Dictionary_%05d.dat", STRINGIFY(TEXTONS_DICTIONARY_PATH), dictionary_number); if ((dictionary_logger = fopen(filename, "r"))) { // Load the dictionary: @@ -501,12 +561,12 @@ void load_texton_dictionary(void) void textons_init(void) { printf("Textons init\n"); - texton_distribution = (float *)calloc(n_textons, sizeof(float)); + texton_distribution = (float *)calloc(MAX_N_TEXTONS, sizeof(float)); dictionary_initialized = 0; learned_samples = 0; dictionary_ready = 0; - dictionary = (float ** **)calloc(n_textons, sizeof(float ** *)); - for (int w = 0; w < n_textons; w++) { + dictionary = (float ** **)calloc(MAX_N_TEXTONS, sizeof(float ** *)); + for (int w = 0; w < MAX_N_TEXTONS; w++) { dictionary[w] = (float ** *) calloc(patch_size, sizeof(float **)); for (int i = 0; i < patch_size; i++) { dictionary[w][i] = (float **) calloc(patch_size, sizeof(float *)); @@ -516,7 +576,7 @@ void textons_init(void) } } - cv_add(texton_func); + listener = cv_add_to_device(&TEXTONS_CAMERA, texton_func, TEXTONS_FPS, 0); } void textons_stop(void) @@ -525,3 +585,20 @@ void textons_stop(void) free(dictionary); } +/** + * Function that calculates a base-2 Shannon entropy for a probability distribution. + * @param[in] p_dist The probability distribution array + * @param[in] D Size of the array + */ +float get_entropy(float *p_dist, int D) +{ + float entropy = 0.0f; + int i; + for (i = 0; i < D; i++) { + if (p_dist[i] > 0) { + entropy -= p_dist[i] * log2(p_dist[i]); + } + } + + return entropy; +} diff --git a/sw/airborne/modules/computer_vision/textons.h b/sw/airborne/modules/computer_vision/textons.h index 693d68dc47..1ab5e489eb 100644 --- a/sw/airborne/modules/computer_vision/textons.h +++ b/sw/airborne/modules/computer_vision/textons.h @@ -39,7 +39,9 @@ extern float *texton_distribution; // main outcome of the image processing: the distribution of textons in the image // settings +extern uint8_t running; extern uint8_t load_dictionary; +extern uint8_t reinitialize_dictionary; extern uint8_t alpha_uint; extern uint8_t n_textons; extern uint8_t patch_size; // Should be even @@ -67,4 +69,7 @@ void load_texton_dictionary(void); extern void textons_init(void); extern void textons_stop(void); +// helper functions (potentially should go elsewhere): +float get_entropy(float *p_dist, int D); + #endif /* TEXTONS_H */ diff --git a/sw/airborne/modules/ctrl/optical_flow_landing.c b/sw/airborne/modules/ctrl/optical_flow_landing.c index 4121f2e524..c8cb6d39cb 100644 --- a/sw/airborne/modules/ctrl/optical_flow_landing.c +++ b/sw/airborne/modules/ctrl/optical_flow_landing.c @@ -39,14 +39,48 @@ * H.W. Ho, G.C.H.E. de Croon, E. van Kampen, Q.P. Chu, and M. Mulder (submitted) * Adaptive Control Strategy for Constant Optical Flow Divergence Landing, * + * + * The module also contains code for learning to map the visual appearance of the landing surface with + * self-supervised learning (SSL) to the corresponding control gains. This leads to smooth optical flow + * landings where the "height" expressed in the control gain is known and can be used to shut down the + * motors. This has been published in the following paper: + * + * de Croon, Guido CHE, Christophe De Wagter, and Tobias Seidl. "Enhancing optical-flow-based control + * by learning visual appearance cues for flying robots." Nature Machine Intelligence 3.1 (2021): 33-41. + * + * Finally, horizontal flow control has been added, so that the drone can hover without optitrack. In fact, + * this module also formed the basis for the experiments in which the attitude is estimated with only optical + * flow and gyro measurements (without accelerometers), published in Nature: + * + * De Croon, G.C.H.E., Dupeyroux, J.J., De Wagter, C., Chatterjee, A., Olejnik, D. A., & Ruffier, F. (2022). + * Accommodating unobservability to control flight attitude with optic flow. Nature, 610(7932), 485-490. + * */ +#include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.h" +#include "math/pprz_algebra_int.h" +#include "math/pprz_algebra_float.h" + #include "optical_flow_landing.h" +// SSL: +float *text_dists[MAX_SAMPLES_LEARNING]; +float sonar_OF[MAX_SAMPLES_LEARNING]; +float gains[MAX_SAMPLES_LEARNING]; +float cov_divs_log[MAX_SAMPLES_LEARNING]; +float *weights; + +// for "communication" with file logger: +float cov_div; +float pstate; +float divergence_vision; + + #include "generated/airframe.h" #include "paparazzi.h" #include "modules/core/abi.h" #include "firmwares/rotorcraft/stabilization.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" #include "firmwares/rotorcraft/guidance/guidance_v_adapt.h" // used for automated landing: @@ -84,6 +118,22 @@ PRINT_CONFIG_VAR(OFL_OPTICAL_FLOW_ID) #define OFL_DGAIN 0.0 #endif +#ifndef OFL_PGAIN_HORIZONTAL_FACTOR +#define OFL_PGAIN_HORIZONTAL_FACTOR 0.05 +#endif + +#ifndef OFL_IGAIN_HORIZONTAL_FACTOR +#define OFL_IGAIN_HORIZONTAL_FACTOR 0.1 +#endif + +#ifndef OFL_ROLL_TRIM +#define OFL_ROLL_TRIM 0.0f +#endif + +#ifndef OFL_PITCH_TRIM +#define OFL_PITCH_TRIM 0.0f +#endif + #ifndef OFL_VISION_METHOD #define OFL_VISION_METHOD 1 #endif @@ -121,20 +171,67 @@ PRINT_CONFIG_VAR(OFL_OPTICAL_FLOW_ID) #define OFL_ELC_OSCILLATE true #endif +#ifndef OFL_CLOSE_TO_EDGE +#define OFL_CLOSE_TO_EDGE 0.025 +#endif + + +#ifndef OFL_PGAIN_ADAPTIVE +#define OFL_PGAIN_ADAPTIVE 0.50 +#endif + +#ifndef OFL_IGAIN_ADAPTIVE +#define OFL_IGAIN_ADAPTIVE 0.50 +#endif + +#ifndef OFL_DGAIN_ADAPTIVE +#define OFL_DGAIN_ADAPTIVE 0.50 +#endif + +#ifndef OFL_OMEGA_LR +#define OFL_OMEGA_LR 0.0 +#endif + +#ifndef OFL_OMEGA_FB +#define OFL_OMEGA_FB 0.0 +#endif + +#ifndef OFL_ACTIVE_MOTION +#define OFL_ACTIVE_MOTION 0 +#endif + +// Normally, horizontal control is done via sending angle commands to INDI, so 0 (false) +// When this is 1 (true),a change in angle will be commanded instead. +#define HORIZONTAL_RATE_CONTROL 0 + +// Normally, ACTIVE_RATES = 0, and the estimated angle is immediately used for controlling the error to 0 +// If ACTIVE_RATES = 1, a sine is actively added to the commanded rates, so that there is often a rate +#define ACTIVE_RATES 0 + // Constants // minimum value of the P-gain for divergence control // adaptive control / exponential gain control will not be able to go lower #define MINIMUM_GAIN 0.1 // for exponential gain landing, gain increase per second during the first (hover) phase: -#define INCREASE_GAIN_PER_SECOND 0.02 +#define INCREASE_GAIN_PER_SECOND 0.10 // variables retained between module calls -float divergence_vision; + +// horizontal loop: +float optical_flow_x; +float optical_flow_y; +float lp_flow_x; +float lp_flow_y; +float sum_roll_error; +float sum_pitch_error; + +//float divergence_vision; float divergence_vision_dt; float normalized_thrust; -float cov_div; -float pstate, pused; +//float cov_div; +//float pstate, pused; +float pused; float istate; float dstate; float vision_time, prev_vision_time; @@ -143,6 +240,38 @@ float previous_cov_err; int32_t thrust_set; float divergence_setpoint; +// ********************************* +// include and define stuff for SSL: +// ********************************* +#define RECURSIVE_LEARNING 0 +#include +#include "modules/computer_vision/textons.h" +// it should now be set to 10 to match the number of textons on the stereoboard... this is extremely ugly. +#define n_ts 10 +float texton_distribution_stereoboard[n_ts]; +// On Bebop 2: +#define TEXTON_DISTRIBUTION_PATH /data/ftp/internal_000 +// for RLS, recursive least squares: +float **P_RLS; +// forgetting factor: +float lambda; +static FILE *distribution_logger = NULL; +static FILE *weights_file = NULL; +unsigned int n_read_samples; +// paparazzi files for doing svd etc.: +#include "math/pprz_algebra_float.h" +#include "math/pprz_matrix_decomp_float.h" +#include "math/pprz_simple_matrix.h" +float module_active_time_sec; +float module_enter_time; +float previous_divergence_setpoint; + +// for ramping +int ramp; +float delta_setpoint; +float ramp_start_time; +float start_setpoint_ramp; + // for the exponentially decreasing gain strategy: int32_t elc_phase; uint32_t elc_time_start; @@ -195,6 +324,7 @@ void vertical_ctrl_module_run(bool in_flight); void vertical_ctrl_module_init(void) { // filling the of_landing_ctrl struct with default values: + of_landing_ctrl.use_bias = true; of_landing_ctrl.agl = 0.0f; of_landing_ctrl.agl_lp = 0.0f; of_landing_ctrl.vel = 0.0f; @@ -206,6 +336,12 @@ void vertical_ctrl_module_init(void) of_landing_ctrl.pgain = OFL_PGAIN; of_landing_ctrl.igain = OFL_IGAIN; of_landing_ctrl.dgain = OFL_DGAIN; + + of_landing_ctrl.pgain_horizontal_factor = OFL_PGAIN_HORIZONTAL_FACTOR; + of_landing_ctrl.igain_horizontal_factor = OFL_IGAIN_HORIZONTAL_FACTOR; + of_landing_ctrl.pitch_trim = OFL_PITCH_TRIM; + of_landing_ctrl.roll_trim = OFL_ROLL_TRIM; + of_landing_ctrl.divergence = 0.; of_landing_ctrl.previous_err = 0.; of_landing_ctrl.sum_err = 0.0f; @@ -216,17 +352,57 @@ void vertical_ctrl_module_init(void) of_landing_ctrl.COV_METHOD = OFL_COV_METHOD; of_landing_ctrl.delay_steps = 15; of_landing_ctrl.window_size = OFL_COV_WINDOW_SIZE; - of_landing_ctrl.pgain_adaptive = OFL_PGAIN; - of_landing_ctrl.igain_adaptive = OFL_IGAIN; - of_landing_ctrl.dgain_adaptive = OFL_DGAIN; + of_landing_ctrl.pgain_adaptive = OFL_PGAIN_ADAPTIVE; + of_landing_ctrl.igain_adaptive = OFL_IGAIN_ADAPTIVE; + of_landing_ctrl.dgain_adaptive = OFL_DGAIN_ADAPTIVE; of_landing_ctrl.reduction_factor_elc = - 0.80f; // for exponential gain landing, after detecting oscillations, the gain is multiplied with this factor + 0.25f; // TODO: it used to be 0.80 for the oscillation landings... make a separate factor for the predictions. + // for exponential gain landing, after detecting oscillations, the gain is multiplied with this factor of_landing_ctrl.lp_cov_div_factor = 0.99f; // low pass filtering cov div so that the drone is really oscillating when triggering the descent of_landing_ctrl.t_transition = 2.f; // if the gain reaches this value during an exponential landing, the drone makes the final landing. of_landing_ctrl.p_land_threshold = OFL_P_LAND_THRESHOLD; of_landing_ctrl.elc_oscillate = OFL_ELC_OSCILLATE; + of_landing_ctrl.close_to_edge = OFL_CLOSE_TO_EDGE; + of_landing_ctrl.lp_factor_prediction = 0.95; + + of_landing_ctrl.omega_FB = OFL_OMEGA_FB; + of_landing_ctrl.omega_LR = OFL_OMEGA_LR; + of_landing_ctrl.active_motion = OFL_ACTIVE_MOTION; + + int i; + if (of_landing_ctrl.use_bias) { + weights = (float *)calloc(n_textons + 1, sizeof(float)); + for (i = 0; i <= n_textons; i++) { + weights[i] = 0.0f; + } + } else { + weights = (float *)calloc(n_textons, sizeof(float)); + for (i = 0; i < n_textons; i++) { + weights[i] = 0.0f; + } + } + + +#ifdef RLS_TEXTONS + // RLS: + P_RLS = (float **)calloc((n_textons + 1), sizeof(float *)); + for (i = 0; i < n_textons + 1; i++) { + P_RLS[i] = (float *)calloc((n_textons + 1), sizeof(float)); + } + int j; + for (i = 0; i < n_textons + 1; i++) { + for (j = 0; j < n_textons + 1; j++) { + if (i == j) { + P_RLS[i][j] = 1.0f; + } else { + P_RLS[i][j] = 0.0f; + } + } + } +#endif + reset_all_vars(); // Subscribe to the altitude above ground level ABI messages @@ -235,6 +411,18 @@ void vertical_ctrl_module_init(void) // register telemetry: AbiBindMsgOPTICAL_FLOW(OFL_OPTICAL_FLOW_ID, &optical_flow_ev, vertical_ctrl_optical_flow_cb); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DIVERGENCE, send_divergence); + + module_enter_time = get_sys_time_float(); + + of_landing_ctrl.ramp_duration = 0.50f; // ramp duration in seconds + // ramping: + ramp = 0; + delta_setpoint = 0.0f; + ramp_start_time = 0.0f; + start_setpoint_ramp = 0.0f; + + lp_flow_x = 0.0f; + lp_flow_y = 0.0f; } /** @@ -242,6 +430,11 @@ void vertical_ctrl_module_init(void) */ static void reset_all_vars(void) { + optical_flow_x = 0.0; + optical_flow_y = 0.0; + sum_roll_error = 0.0; + sum_pitch_error = 0.0; + of_landing_ctrl.agl_lp = of_landing_ctrl.agl = stateGetPositionEnu_f()->z; thrust_set = of_landing_ctrl.nominal_thrust * MAX_PPRZ; @@ -252,6 +445,7 @@ static void reset_all_vars(void) divergence_vision = 0.; divergence_vision_dt = 0.; divergence_setpoint = 0; + previous_divergence_setpoint = divergence_setpoint; vision_time = get_sys_time_float(); prev_vision_time = vision_time; @@ -276,10 +470,20 @@ static void reset_all_vars(void) istate = of_landing_ctrl.igain; dstate = of_landing_ctrl.dgain; + of_landing_ctrl.load_weights = true; of_landing_ctrl.divergence = 0.; of_landing_ctrl.previous_err = 0.; of_landing_ctrl.sum_err = 0.; of_landing_ctrl.d_err = 0.; + + // ramping: + ramp = 0; + delta_setpoint = 0.0f; + ramp_start_time = 0.0f; + start_setpoint_ramp = 0.0f; + + lp_flow_x = 0.0f; + lp_flow_y = 0.0f; } /** @@ -296,13 +500,45 @@ void vertical_ctrl_module_run(bool in_flight) return; } + module_active_time_sec = get_sys_time_float() - module_enter_time; + Bound(of_landing_ctrl.lp_const, 0.001f, 1.f); float lp_factor = dt / of_landing_ctrl.lp_const; Bound(lp_factor, 0.f, 1.f); + if (of_landing_ctrl.load_weights) { + printf("LOADING WEIGHTS!\n"); + load_weights(); + of_landing_ctrl.load_weights = false; + } + + if (!in_flight) { + + // When not flying and in mode module: + // Reset integrators, landing phases, etc. + // reset_all_vars(); // commented out to allow us to study the observation variables in-hand, i.e., without flying + + + // SSL: only learn if not flying - due to use of resources: + if (of_landing_ctrl.learn_gains) { + printf("LEARNING WEIGHTS!\n"); + float start = get_sys_time_float(); + // learn the weights from the file filled with training examples: + learn_from_file(); + + float end = get_sys_time_float(); + printf("Time to learn: %f\n", end - start); + // reset the learn_gains variable to false: + of_landing_ctrl.learn_gains = false; + // dt is smaller than it actually should be... + } + } + /*********** * VISION ***********/ + float new_flow_x = 0.0; + float new_flow_y = 0.0; if (of_landing_ctrl.VISION_METHOD == 0) { // SIMULATED DIVERGENCE: @@ -334,8 +570,11 @@ void vertical_ctrl_module_run(bool in_flight) // TODO: this factor is camera specific and should be implemented in the optical // flow calculator module not here. Additionally, the time scaling should also // be done in the calculator module - div_factor = -1.28f; // magic number comprising field of view etc. + // div_factor = -1.28f; // magic number comprising field of view etc. + div_factor = -2.25f; // in the sim float new_divergence = (divergence_vision * div_factor) / dt; + new_flow_x = optical_flow_x / dt; + new_flow_y = optical_flow_y / dt; // deal with (unlikely) fast changes in divergence: static const float max_div_dt = 0.20f; @@ -354,6 +593,20 @@ void vertical_ctrl_module_run(bool in_flight) ***********/ // landing indicates whether the drone is already performing a final landing procedure (flare): if (!landing) { + + /* + // First seconds, don't do anything crazy: + if (module_active_time_sec < 2.5f) { + int32_t nominal_throttle = of_landing_ctrl.nominal_thrust * MAX_PPRZ; + thrust_set = nominal_throttle; + stabilization_cmd[COMMAND_THRUST] = thrust; + // keep track of histories and set the covariance + set_cov_div(thrust_set); + cov_div = 0.0f; // however, cov div is set to 0 to prevent strange issues at module startup. + return; + } + */ + if (of_landing_ctrl.CONTROL_METHOD == 0) { // FIXED GAIN CONTROL, cov_limit for landing: @@ -386,6 +639,12 @@ void vertical_ctrl_module_run(bool in_flight) thrust_set = PID_divergence_control(of_landing_ctrl.divergence_setpoint, pused, of_landing_ctrl.igain, of_landing_ctrl.dgain, dt); + // SSL: if close enough, store texton inputs: + printf("abs error cov = %f, close = %f\n", fabs(error_cov), of_landing_ctrl.close_to_edge); + if (fabs(error_cov) < of_landing_ctrl.close_to_edge) { + save_texton_distribution(); + } + // when to make the final landing: if (pstate < of_landing_ctrl.p_land_threshold) { thrust_set = final_landing_procedure(); @@ -486,14 +745,225 @@ void vertical_ctrl_module_run(bool in_flight) } else { thrust_set = final_landing_procedure(); } + } else if (of_landing_ctrl.CONTROL_METHOD == 3) { + + // SSL LANDING: use learned weights for setting the gain on the way down: + + // adapt the p-gain with a low-pass filter to the gain predicted by image appearance: + pstate = predict_gain(texton_distribution); + // low-pass filtered, downscaled pstate predictions: + // TODO: just as with divergence, also take dt into account for low-pass filtering: + of_landing_ctrl.pgain = of_landing_ctrl.lp_factor_prediction * of_landing_ctrl.pgain + + (1.0f - of_landing_ctrl.lp_factor_prediction) * of_landing_ctrl.reduction_factor_elc * pstate; + pused = of_landing_ctrl.pgain; + // make sure pused does not become too small, nor grows too fast: + if (of_landing_ctrl.pgain < MINIMUM_GAIN) { of_landing_ctrl.pgain = MINIMUM_GAIN; } + // have the i and d gain depend on the p gain: + istate = 0.025 * of_landing_ctrl.pgain; + dstate = 0.0f; + printf("of_landing_ctrl.pgain = %f, divergence = %f, phase = %d\n", of_landing_ctrl.pgain, of_landing_ctrl.divergence, + elc_phase); + + if (elc_phase == 0) { + // Let the low-pass filter settle first with 0 divergence: + float phase_0_set_point = 0.0f; + previous_divergence_setpoint = 0.0f; + + // use the divergence for control: + thrust_set = PID_divergence_control(phase_0_set_point, pused, istate, dstate, dt); + + // if the low-pass filter of the p-gain has settled and the drone is moving in the right direction: + if (module_active_time_sec >= 3.0f && of_landing_ctrl.divergence * of_landing_ctrl.divergence_setpoint >= 0.0f) { + // next phase: + elc_phase = 1; + } + } else { + // use the chosen divergence setpoint and the measured divergence for control: + thrust_set = PID_divergence_control(of_landing_ctrl.divergence_setpoint, pused, istate, dstate, dt); + } + if (pused < of_landing_ctrl.p_land_threshold) { + thrust_set = final_landing_procedure(); + } + } else if (of_landing_ctrl.CONTROL_METHOD == 4) { + + // Mixed SSL + EXPONENTIAL LANDING: use learned weights for setting the gain on the way down: + + if (elc_phase == 0) { + + float phase_0_set_point = 0.0f; + previous_divergence_setpoint = 0.0f; + // adapt the p-gain with a low-pass filter to the gain predicted by image appearance: + pstate = predict_gain(texton_distribution); + // of_landing_ctrl.pgain = lp_factor_prediction * of_landing_ctrl.pgain + (1.0f - lp_factor_prediction) * of_landing_ctrl.stable_gain_factor * pstate; + of_landing_ctrl.pgain = of_landing_ctrl.lp_factor_prediction * of_landing_ctrl.pgain + + (1.0f - of_landing_ctrl.lp_factor_prediction) * of_landing_ctrl.reduction_factor_elc * pstate; + pused = of_landing_ctrl.pgain; + // make sure pused does not become too small, nor grows too fast: + if (of_landing_ctrl.pgain < MINIMUM_GAIN) { of_landing_ctrl.pgain = MINIMUM_GAIN; } + // have the i and d gain depend on the p gain: + istate = 0.025 * of_landing_ctrl.pgain; + dstate = 0.0f; + printf("of_landing_ctrl.pgain = %f, divergence = %f\n", of_landing_ctrl.pgain, of_landing_ctrl.divergence); + + // use the divergence for control: + thrust_set = PID_divergence_control(phase_0_set_point, pused, istate, dstate, dt); + + // if the low-pass filter of the p-gain has settled and the drone is moving in the right direction: + if (module_active_time_sec >= 3.0f && of_landing_ctrl.divergence * of_landing_ctrl.divergence_setpoint >= 0.0f) { + // next phase: + elc_phase = 1; + elc_time_start = get_sys_time_float(); + + // we don't have to reduce the gain with of_landing_ctrl.reduction_factor_elc, as this is done above for the p-gain already: + elc_p_gain_start = pused; + elc_i_gain_start = istate; + elc_d_gain_start = dstate; + count_covdiv = 0; + of_landing_ctrl.sum_err = 0.0f; + } + } else if (elc_phase == 1) { + // land while exponentially decreasing the gain: + float new_time = get_sys_time_float(); + float t_interval = new_time - elc_time_start; + + // this should not happen, but just to be sure to prevent too high gain values: + if (t_interval < 0) { t_interval = 0.0f; } + + // determine the P-gain, weighing between an exponentially decaying one and the predicted one based on textons: + float gain_scaling = exp(of_landing_ctrl.divergence_setpoint * t_interval); + float prediction = predict_gain(texton_distribution); // low-pass it in of_landing_ctrl.pgain? + float weight_prediction = 0.5; + + if (gain_scaling <= 1.0f) { + pstate = weight_prediction * of_landing_ctrl.reduction_factor_elc * prediction + (1 - weight_prediction) * + elc_p_gain_start * gain_scaling; + istate = 0.025 * weight_prediction * of_landing_ctrl.reduction_factor_elc * prediction + + (1 - weight_prediction) * elc_i_gain_start * gain_scaling; + dstate = 0.0f; + } else { + // should never come here: + pstate = of_landing_ctrl.reduction_factor_elc * prediction; + istate = 0.025 * of_landing_ctrl.reduction_factor_elc * prediction; + dstate = 0.0f; + } + pused = pstate; + + // use the divergence for control: + thrust_set = PID_divergence_control(of_landing_ctrl.divergence_setpoint, pused, istate, dstate, dt); + + // when to make the final landing: + if (pstate < of_landing_ctrl.p_land_threshold) { + elc_phase = 2; + } + } else { + thrust_set = final_landing_procedure(); + } + } else if (of_landing_ctrl.CONTROL_METHOD == 5) { + + // SSL raw, no landing procedure, ideal for hovering: + + pstate = predict_gain(texton_distribution); + // printf("prediction = %f\n", pstate); + of_landing_ctrl.pgain = of_landing_ctrl.lp_factor_prediction * of_landing_ctrl.pgain + + (1.0f - of_landing_ctrl.lp_factor_prediction) * of_landing_ctrl.reduction_factor_elc * pstate; + pused = of_landing_ctrl.pgain; + // make sure pused does not become too small, nor grows too fast: + if (of_landing_ctrl.pgain < MINIMUM_GAIN) { of_landing_ctrl.pgain = MINIMUM_GAIN; } + // have the i and d gain depend on the p gain: + istate = 0.025 * of_landing_ctrl.pgain; + dstate = 0.0f; + printf("of_landing_ctrl.pgain = %f\n", of_landing_ctrl.pgain); + + // use the divergence for control: + thrust_set = PID_divergence_control(of_landing_ctrl.divergence_setpoint, pused, istate, dstate, dt); + + if (pstate < of_landing_ctrl.p_land_threshold) { + thrust_set = final_landing_procedure(); + } } if (in_flight) { Bound(thrust_set, 0.25 * of_landing_ctrl.nominal_thrust * MAX_PPRZ, MAX_PPRZ); stabilization_cmd[COMMAND_THRUST] = thrust_set; } - } + + /*********************/ + // Horizontal control: + /*********************/ + + struct FloatEulers *attitude = stateGetNedToBodyEulers_f(); + + // negative command is flying forward, positive back. + lp_flow_x = of_landing_ctrl.lp_factor_prediction * lp_flow_x + (1.0f - of_landing_ctrl.lp_factor_prediction) * + optical_flow_x; + lp_flow_y = of_landing_ctrl.lp_factor_prediction * lp_flow_y + (1.0f - of_landing_ctrl.lp_factor_prediction) * + optical_flow_y; + + + if (of_landing_ctrl.active_motion == 1) { + // Active motion through varying ventral flow commands + float period_factor = 0.2; + float sine_threshold = 0.75; + float omega_set_point = 20; + + if (sinf(period_factor * module_active_time_sec) > sine_threshold) { + of_landing_ctrl.omega_LR = omega_set_point; + } else if (sinf(period_factor * module_active_time_sec) < -sine_threshold) { + of_landing_ctrl.omega_LR = -omega_set_point; + } else { + of_landing_ctrl.omega_LR = 0.0f; + } + } else if (of_landing_ctrl.active_motion == 2) { + // Active motion through varying roll trim commands + float period_factor = 0.2; + float sine_threshold = 0.9; + float trim_set_point = 2.0f; + + if (sinf(period_factor * module_active_time_sec) > sine_threshold) { + of_landing_ctrl.roll_trim = trim_set_point; + } else if (sinf(period_factor * module_active_time_sec) < -sine_threshold) { + of_landing_ctrl.roll_trim = -trim_set_point; + } else { + of_landing_ctrl.roll_trim = 0.0f; + } + } + + float error_pitch = new_flow_y - of_landing_ctrl.omega_FB; + float error_roll = new_flow_x - of_landing_ctrl.omega_LR; + sum_pitch_error += error_pitch; + sum_roll_error += error_roll; + float P_hor = of_landing_ctrl.pgain_horizontal_factor * of_landing_ctrl.pgain; + float I_hor = of_landing_ctrl.igain_horizontal_factor * of_landing_ctrl.igain; + float pitch_cmd; + float roll_cmd; + float add_active_sine = 0.0f; + + if (ACTIVE_RATES) { + float sine_period = 0.05; + float sine_amplitude = RadOfDeg(1.0); + float sine_time = get_sys_time_float(); + add_active_sine = sine_amplitude * sinf((1.0f / sine_period) * sine_time * 2 * M_PI); + printf("add_active_sine = %f\n", add_active_sine); + } + if (!HORIZONTAL_RATE_CONTROL) { + // normal operation: + pitch_cmd = RadOfDeg(of_landing_ctrl.pitch_trim + error_pitch * P_hor + I_hor * sum_pitch_error); + // add_active_sine = 0.0f in normal operation: + roll_cmd = RadOfDeg(of_landing_ctrl.roll_trim + error_roll * P_hor + I_hor * sum_roll_error) + add_active_sine; + } + BoundAbs(pitch_cmd, RadOfDeg(10.0)); + BoundAbs(roll_cmd, RadOfDeg(10.0)); + float psi_cmd = + attitude->psi; // control of psi in simulation is still a bit enthusiastic! Increase the associated control effectiveness. + struct Int32Eulers rpy = { .phi = (int32_t)ANGLE_BFP_OF_REAL(roll_cmd), + .theta = (int32_t)ANGLE_BFP_OF_REAL(pitch_cmd), .psi = (int32_t)ANGLE_BFP_OF_REAL(psi_cmd) + }; + + // set the desired roll pitch and yaw: + stabilization_indi_set_rpy_setpoint_i(&rpy); + // execute attitude stabilization: + stabilization_attitude_run(in_flight); } /** @@ -554,8 +1024,36 @@ void set_cov_div(int32_t thrust) */ int32_t PID_divergence_control(float setpoint, float P, float I, float D, float dt) { - // determine the error: - float err = setpoint - of_landing_ctrl.divergence; + + if (previous_divergence_setpoint != setpoint) { + ramp = 1; + delta_setpoint = setpoint - previous_divergence_setpoint; + start_setpoint_ramp = previous_divergence_setpoint; + ramp_start_time = get_sys_time_float(); + } + + // determine the error, possibly using a ramp: + float err; + if (!ramp) { + err = setpoint - of_landing_ctrl.divergence; + } else { + // rt is in [0,1] and used to make the ramp: + float ramp_time = get_sys_time_float(); + float rt = (ramp_time - ramp_start_time) / (of_landing_ctrl.ramp_duration); + + if (rt > 1.0f) { + ramp = 0; // end of the ramp + err = setpoint - of_landing_ctrl.divergence; + // Send the setpoint back via the divergence message, so that we can see that this procedure works + divergence_vision_dt = setpoint; + } else { + float ds = start_setpoint_ramp + rt * delta_setpoint; + // Send the setpoint back via the divergence message, so that we can see that this procedure works + divergence_vision_dt = ds; + // determine the error: + err = ds - of_landing_ctrl.divergence; + } + } // update the controller errors: update_errors(err, dt); @@ -572,6 +1070,8 @@ int32_t PID_divergence_control(float setpoint, float P, float I, float D, float // update covariance set_cov_div(thrust); + previous_divergence_setpoint = setpoint; + return thrust; } @@ -597,16 +1097,44 @@ void vertical_ctrl_agl_cb(uint8_t sender_id UNUSED, __attribute__((unused)) uint of_landing_ctrl.agl = distance; } -void vertical_ctrl_optical_flow_cb(uint8_t sender_id UNUSED, uint32_t stamp, +void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x UNUSED, int32_t flow_y UNUSED, - int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED, float quality UNUSED, float size_divergence) + int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED, + float quality UNUSED, float size_divergence) { - divergence_vision = size_divergence; - vision_time = ((float)stamp) / 1e6; + + if (sender_id == FLOW_OPTICFLOW_ID + 0) { // 0 = bottom 1 = front + optical_flow_x = ((float)flow_der_x) / 10.0; + optical_flow_y = ((float)flow_der_y) / 10.0; + divergence_vision = size_divergence; + //printf("Reading %f, %f, %f\n", optical_flow_x, optical_flow_y, divergence_vision); + vision_time = ((float)stamp) / 1e6; + } } //////////////////////////////////////////////////////////////////// // Call our controller + +void guidance_h_module_init(void) +{ + +} + +void guidance_h_module_enter(void) +{ + +} + +void guidance_h_module_run(bool in_flight) +{ + +} + +void guidance_h_module_read_rc(void) +{ + +} + void guidance_v_module_init(void) { vertical_ctrl_module_init(); @@ -628,3 +1156,358 @@ void guidance_v_module_run(bool in_flight) { vertical_ctrl_module_run(in_flight); } + +// SSL: +void save_texton_distribution(void) +{ + printf("Logging textons!\n"); + int i; + + // If not the same, append the target values (heights, gains) and texton values to a .dat file: + char filename[512]; + sprintf(filename, "%s/Training_set_%05d.dat", STRINGIFY(TEXTON_DISTRIBUTION_PATH), 0); + distribution_logger = fopen(filename, "a"); + if (distribution_logger == NULL) { + perror(filename); + //perror("Error while opening the file.\n"); + } else { + printf("Logging at height %f, gain %f, cov_div %f\n", of_landing_ctrl.agl, pstate, cov_div); + + // save the information in a single row: + fprintf(distribution_logger, "%f ", of_landing_ctrl.agl); // sonar measurement + fprintf(distribution_logger, "%f ", pstate); // gain measurement + fprintf(distribution_logger, "%f ", cov_div); // cov div measurement + for (i = 0; i < n_textons - 1; i++) { + fprintf(distribution_logger, "%f ", texton_distribution[i]); + } + fprintf(distribution_logger, "%f\n", texton_distribution[n_textons - 1]); + fclose(distribution_logger); + } +} + +void load_texton_distribution(void) +{ + int i, j, read_result; + char filename[512]; + + sprintf(filename, "%s/Training_set_%05d.dat", STRINGIFY(TEXTON_DISTRIBUTION_PATH), 0); + printf("Loading textons from %s\n", filename); + + distribution_logger = fopen(filename, "r"); + if (distribution_logger) { + // Load the dictionary: + n_read_samples = 0; + // For now we read the samples sequentially: + for (i = 0; i < MAX_SAMPLES_LEARNING; i++) + //for(i = 0; i < 30; i++) + { + read_result = fscanf(distribution_logger, "%f ", &sonar_OF[n_read_samples]); + if (read_result == EOF) { break; } + read_result = fscanf(distribution_logger, "%f ", &gains[n_read_samples]); + if (read_result == EOF) { break; } + read_result = fscanf(distribution_logger, "%f ", &cov_divs_log[n_read_samples]); + if (read_result == EOF) { break; } + + text_dists[n_read_samples] = (float *) calloc(n_textons, sizeof(float)); + for (j = 0; j < n_textons - 1; j++) { + read_result = fscanf(distribution_logger, "%f ", &text_dists[n_read_samples][j]); + if (read_result == EOF) { break; } + } + read_result = fscanf(distribution_logger, "%f\n", &text_dists[n_read_samples][n_textons - 1]); + if (read_result == EOF) { break; } + n_read_samples++; + } + fclose(distribution_logger); + } else { + printf("There was an error opening %s\n", filename); + } +} + +void learn_from_file(void) +{ + int i; + float fit_error; + + // first load the texton distributions: + printf("Loading distribution\n"); + load_texton_distribution(); + + // then learn from it: + printf("Learning!\n"); + if (!RECURSIVE_LEARNING) { + fit_linear_model_OF(gains, text_dists, n_textons, n_read_samples, weights, &fit_error); + // fit_linear_model_OF(sonar_OF, text_dists, n_textons, n_read_samples, weights, &fit_error); + } else { + recursive_least_squares_batch(gains, text_dists, n_textons, n_read_samples, weights, &fit_error); + } + + printf("Saving!\n"); + // save the weights to a file: + save_weights(); + + printf("Learned! Fit error = %f\n", fit_error); + + // free learning distributions: + for (i = 0; i < n_read_samples; i++) { + free(text_dists[i]); + } +} + +/** + * Recursively fit a linear model from samples to target values - batch mode, possibly for initialization. + * @param[in] targets The target values + * @param[in] samples The samples / feature vectors + * @param[in] D The dimensionality of the samples + * @param[in] count The number of samples + * @param[out] parameters* Parameters of the linear fit + * @param[out] fit_error* Total error of the fit // TODO: relevant for RLS? + */ +void recursive_least_squares_batch(float *targets, float **samples, uint8_t D, uint16_t count, float *params, + float *fit_error) +{ + // TODO: potentially randomizing the sequence of the samples, as not to get a bias towards the later samples + // local vars for iterating, random numbers: + int sam, d; + uint8_t D_1 = D + 1; + float augmented_sample[D_1]; + // augmented sample is used if the bias is used: + augmented_sample[D] = 1.0f; + + // Initialize the weights with 0.0f: + for (d = 0; d < D_1; d++) { + weights[d] = 0.0f; + } + + // Reset the P matrix to an identity matrix: + int i, j; + for (i = 0; i < n_textons + 1; i++) { + for (j = 0; j < n_textons + 1; j++) { + if (i == j) { + P_RLS[i][j] = 1.0f; + } else { + P_RLS[i][j] = 0.0f; + } + } + } + + // give the samples one by one to the recursive procedure: + for (sam = 0; sam < count; sam++) { + if (!of_landing_ctrl.use_bias) { + recursive_least_squares(targets[sam], samples[sam], D, params); + } else { + for (d = 0; d < D; d++) { + augmented_sample[d] = samples[sam][d]; + } + recursive_least_squares(targets[sam], augmented_sample, D_1, params); + } + } + + // give the samples one by one to the recursive procedure to determine the fit on the training set: + float prediction; + float sum_abs_err = 0; + for (sam = 0; sam < count; sam++) { + prediction = predict_gain(samples[sam]); + sum_abs_err += fabs(prediction - targets[sam]); + } + (*fit_error) = sum_abs_err / count; +} + +void recursive_least_squares(float target, float *sample, uint8_t length_sample, float *params) +{ + // MATLAB procedure: + /* + u = features(i,:); + phi = u * P ; + k(:, i) = phi' / (lamda + phi * u' ); + y(i)= weights(:,i)' * u'; + e(i) = targets(i) - y(i) ; + weights(:,i+1) = weights(:,i) + k(:, i) * e(i) ; + P = ( P - k(:, i) * phi ) / lamda; + */ + + float _u[1][length_sample]; + float _uT[length_sample][1]; + float _phi[1][length_sample]; + float _phiT[length_sample][1]; + float _k[length_sample][1]; + float _ke[length_sample][1]; + float prediction, error; + float _u_P_uT[1][1]; + float scalar; + float _O[length_sample][length_sample]; + int i; + + // u = features(i,:); + for (i = 0; i < length_sample; i++) { + _u[0][i] = sample[i]; + _uT[i][0] = sample[i]; + } + MAKE_MATRIX_PTR(u, _u, 1); + MAKE_MATRIX_PTR(uT, _uT, length_sample); // transpose + MAKE_MATRIX_PTR(phi, _phi, 1); + MAKE_MATRIX_PTR(phiT, _phiT, length_sample); + MAKE_MATRIX_PTR(u_P_uT, _u_P_uT, 1); // actually a scalar + MAKE_MATRIX_PTR(k, _k, length_sample); + MAKE_MATRIX_PTR(ke, _ke, length_sample); + + MAKE_MATRIX_PTR(P, P_RLS, length_sample); + MAKE_MATRIX_PTR(O, _O, length_sample); + // phi = u * P ; + // result of multiplication goes into phi + MAT_MUL(1, length_sample, length_sample, phi, u, P); + // k(:, i) = phi' / (lamda + phi * u' ); + for (i = 0; i < length_sample; i++) { + phiT[i][0] = phi[0][i]; + } + // scalar: + MAT_MUL(1, length_sample, 1, u_P_uT, phi, uT); + scalar = u_P_uT[0][0]; + float_mat_div_scalar(k, phiT, lambda + scalar, length_sample, 1); + // y(i)= weights(:,i)' * u'; + prediction = predict_gain(sample); + // e(i) = targets(i) - y(i) ; + error = target - prediction; + // weights(:,i+1) = weights(:,i) + k(:, i) * e(i) ; + float_mat_mul_scalar(ke, k, error, length_sample, 1); + for (i = 0; i < length_sample; i++) { + weights[i] += ke[i][0]; + } + // P = ( P - k(:, i) * phi ) / lamda; + MAT_MUL(length_sample, 1, length_sample, O, k, phi); + MAT_SUB(length_sample, length_sample, P, P, O); + float_mat_div_scalar(P, P, lambda, length_sample, length_sample); +} + +/** + * Fit a linear model from samples to target values. + * @param[in] targets The target values + * @param[in] samples The samples / feature vectors + * @param[in] D The dimensionality of the samples + * @param[in] count The number of samples + * @param[out] parameters* Parameters of the linear fit + * @param[out] fit_error* Total error of the fit + */ +void fit_linear_model_OF(float *targets, float **samples, uint8_t D, uint16_t count, float *params, float *fit_error) +{ + + // We will solve systems of the form A x = b, + // where A = [nx(D+1)] matrix with entries [s1, ..., sD, 1] for each sample (1 is the bias) + // and b = [nx1] vector with the target values. + // x in the system are the parameters for the linear regression function. + + // local vars for iterating, random numbers: + int sam, d; + uint16_t n_samples = count; + uint8_t D_1 = D + 1; + // ensure that n_samples is high enough to ensure a result for a single fit: + n_samples = (n_samples < D_1) ? D_1 : n_samples; + // n_samples should not be higher than count: + n_samples = (n_samples < count) ? n_samples : count; + + // initialize matrices and vectors for the full point set problem: + // this is used for determining inliers + float _AA[count][D_1]; + MAKE_MATRIX_PTR(AA, _AA, count); + float _targets_all[count][1]; + MAKE_MATRIX_PTR(targets_all, _targets_all, count); + + for (sam = 0; sam < count; sam++) { + for (d = 0; d < D; d++) { + AA[sam][d] = samples[sam][d]; + } + if (of_landing_ctrl.use_bias) { + AA[sam][D] = 1.0f; + } else { + AA[sam][D] = 0.0f; + } + targets_all[sam][0] = targets[sam]; + } + + + // decompose A in u, w, v with singular value decomposition A = u * w * vT. + // u replaces A as output: + float _parameters[D_1][1]; + MAKE_MATRIX_PTR(parameters, _parameters, D_1); + float w[n_samples], _v[D_1][D_1]; + MAKE_MATRIX_PTR(v, _v, D_1); + + pprz_svd_float(AA, w, v, count, D_1); + pprz_svd_solve_float(parameters, AA, w, v, targets_all, count, D_1, 1); + + // used to determine the error of a set of parameters on the whole set: + float _bb[count][1]; + MAKE_MATRIX_PTR(bb, _bb, count); + float _C[count][1]; + MAKE_MATRIX_PTR(C, _C, count); + + // error is determined on the entire set + // bb = AA * parameters: + MAT_MUL(count, D_1, 1, bb, AA, parameters); + // subtract bu_all: C = 0 in case of perfect fit: + MAT_SUB(count, 1, C, bb, targets_all); + *fit_error = 0; + for (sam = 0; sam < count; sam++) { + *fit_error += fabs(C[sam][0]); + } + *fit_error /= count; + + for (d = 0; d < D_1; d++) { + params[d] = parameters[d][0]; + } + +} + + +float predict_gain(float *distribution) +{ + //printf("Start prediction!\n"); + int i; + float sum; + + sum = 0.0f; + for (i = 0; i < n_textons; i++) { + sum += weights[i] * distribution[i]; + } + if (of_landing_ctrl.use_bias) { + sum += weights[n_textons]; + } + // printf("Prediction = %f\n", sum); + return sum; +} + +void save_weights(void) +{ + // save the weights to a file: + int i; + char filename[512]; + sprintf(filename, "%s/Weights_%05d.dat", STRINGIFY(TEXTON_DISTRIBUTION_PATH), 0); + weights_file = fopen(filename, "w"); + if (weights_file == NULL) { + perror(filename); + } else { + // save the information in a single row: + for (i = 0; i <= n_textons; i++) { + fprintf(weights_file, "%f ", weights[i]); + } + fclose(weights_file); + } +} + +void load_weights(void) +{ + int i, read_result; + char filename[512]; + sprintf(filename, "%s/Weights_%05d.dat", STRINGIFY(TEXTON_DISTRIBUTION_PATH), 0); + weights_file = fopen(filename, "r"); + if (weights_file == NULL) { + printf("No weights file!\n"); + perror(filename); + } else { + // load the weights, stored in a single row: + for (i = 0; i <= n_textons; i++) { + read_result = fscanf(weights_file, "%f ", &weights[i]); + if (read_result == EOF) { break; } + } + fclose(weights_file); + } +} diff --git a/sw/airborne/modules/ctrl/optical_flow_landing.h b/sw/airborne/modules/ctrl/optical_flow_landing.h index df45188f4a..1ceb89e44d 100644 --- a/sw/airborne/modules/ctrl/optical_flow_landing.h +++ b/sw/airborne/modules/ctrl/optical_flow_landing.h @@ -46,10 +46,13 @@ #include "std.h" +// maximum number of samples to learn from for SSL: +#define MAX_SAMPLES_LEARNING 25000 + struct OpticalFlowLanding { float agl; ///< agl = height from sonar (only used when using "fake" divergence) float agl_lp; ///< low-pass version of agl - float lp_const; ///< low-pass filter constant + float lp_const; ///< low-pass value for divergence float vel; ///< vertical velocity as determined with sonar (only used when using "fake" divergence) float divergence_setpoint; ///< setpoint for constant divergence approach float pgain; ///< P-gain for constant divergence control (from divergence error to thrust) @@ -61,7 +64,7 @@ struct OpticalFlowLanding { float d_err; ///< difference of error for the D-gain float nominal_thrust; ///< nominal thrust around which the PID-control operates uint32_t VISION_METHOD; ///< whether to use vision (1) or Optitrack / sonar (0) - uint32_t CONTROL_METHOD; ///< type of divergence control: 0 = fixed gain, 1 = adaptive gain + uint32_t CONTROL_METHOD; ///< type of divergence control: 0 = fixed gain, 1 = adaptive gain, 2 = exponential time landing control. 3 = learning control float cov_set_point; ///< for adaptive gain control, setpoint of the covariance (oscillations) float cov_limit; ///< for fixed gain control, what is the cov limit triggering the landing float pgain_adaptive; ///< P-gain for adaptive gain control @@ -75,20 +78,57 @@ struct OpticalFlowLanding { float t_transition; ///< how many seconds the drone has to be oscillating in order to transition to the hover phase with reduced gain float p_land_threshold; ///< if during the exponential landing the gain reaches this value, the final landing procedure is triggered bool elc_oscillate; ///< Whether or not to oscillate at beginning of elc to find optimum gain + // Added features for SSL: + volatile bool + learn_gains; ///< set to true if the robot needs to learn a mapping from texton distributions to the p-gain + bool load_weights; ///< load the weights that were learned before + float close_to_edge; ///< if abs(cov_div - reference cov_div) < close_to_edge, then texton distributions and gains are stored for learning + bool use_bias; ///< if true, a bias 1.0f will be added to the feature vector (texton distribution) - this typically leads to very large weights + bool snapshot; ///< if true, besides storing a texton distribution, an image will also be stored (when unstable) + float lp_factor_prediction; ///< low-pass value for the predicted P-value + float ramp_duration; ///< ramp duration in seconds for when the divergence setpoint changes + float pgain_horizontal_factor;///< factor multiplied with the vertical P-gain for horizontal ventral-flow-based control + float igain_horizontal_factor;///< factor multiplied with the vertical I-gain for horizontal ventral-flow-based control + float roll_trim; ///< Roll trim angle in degrees + float pitch_trim; ///< Pitch trim angle in degrees + float omega_LR; ///< Set point for the left-right ventral flow + float omega_FB; ///< Set point for the front-back ventral flow + uint32_t active_motion; ///< Method for actively inducing motion }; extern struct OpticalFlowLanding of_landing_ctrl; + // Without optitrack set to: GUIDANCE_H_MODE_ATTITUDE -// With optitrack set to: GUIDANCE_H_MODE_HOVER / NAV -#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_NAV +// With optitrack set to: GUIDANCE_H_MODE_HOVER / NAV (NAV is the common option in the experiments.) + +#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE // Own guidance_v #define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE +// Implement own horizontal loop: +extern void guidance_h_module_init(void); +extern void guidance_h_module_enter(void); +extern void guidance_h_module_run(bool in_flight); +extern void guidance_h_module_read_rc(void); + // Implement own Vertical loops extern void guidance_v_module_init(void); extern void guidance_v_module_enter(void); extern void guidance_v_module_run(bool in_flight); +// SSL functions: +void save_texton_distribution(void); +void load_texton_distribution(void); +void fit_linear_model_OF(float *targets, float **samples, uint8_t D, uint16_t count, float *parameters, + float *fit_error); +void learn_from_file(void); +float predict_gain(float *distribution); +void save_weights(void); +void load_weights(void); +void recursive_least_squares_batch(float *targets, float **samples, uint8_t D, uint16_t count, float *params, + float *fit_error); +void recursive_least_squares(float target, float *sample, uint8_t length_sample, float *params); + #endif /* OPTICAL_FLOW_LANDING_H_ */ diff --git a/sw/airborne/modules/ins/ins_flow.c b/sw/airborne/modules/ins/ins_flow.c new file mode 100644 index 0000000000..2ea191977d --- /dev/null +++ b/sw/airborne/modules/ins/ins_flow.c @@ -0,0 +1,1571 @@ +/* + * Copyright (C) 2021 Guido de Croon + * + * 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. + */ + +/** + * @file subsystems/ins/ins_flow.c + * + * INS_flow estimates the attitude (and can also estimate velocity and height), based on optical flow and gyro + * measurements alone. This module is inspired by the fact that flying insects do not have accelerometers. It + * formed the basis for the experiments in which the attitude is estimated with only optical + * flow and gyro measurements (without accelerometers), published in Nature: + * + * De Croon, G.C.H.E., Dupeyroux, J.J., De Wagter, C., Chatterjee, A., Olejnik, D. A., & Ruffier, F. (2022). + * Accommodating unobservability to control flight attitude with optic flow. Nature, 610(7932), 485-490. + * https://www.nature.com/articles/s41586-022-05182-2 + * + */ + +#include "ins_flow.h" +#include "modules/core/abi.h" +#include "generated/airframe.h" +#include "mcu_periph/sys_time.h" +#include "autopilot.h" +#include "math/pprz_algebra_float.h" +#include "generated/airframe.h" +#include "generated/flight_plan.h" +#include "mcu_periph/sys_time.h" +#include "modules/actuators/motor_mixing.h" +#include + +#define DEBUG_INS_FLOW 0 +#if DEBUG_INS_FLOW +#include +#include "math/pprz_simple_matrix.h" +#define DEBUG_PRINT printf +#define DEBUG_MAT_PRINT MAT_PRINT +//#define DEBUG_MAT_PRINT(...) +#else +#define DEBUG_PRINT(...) +#define DEBUG_MAT_PRINT(...) +#endif + +#ifndef AHRS_ICQ_OUTPUT_ENABLED +#define AHRS_ICQ_OUTPUT_ENABLED TRUE +#endif +PRINT_CONFIG_VAR(AHRS_ICQ_OUTPUT_ENABLED) + +/* default Gyro to use in INS */ +#ifndef INS_FLOW_GYRO_ID +#define INS_FLOW_GYRO_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(INS_FLOW_GYRO_ID) + +/* default Accelerometer to use in INS */ +#ifndef INS_FLOW_ACCEL_ID +#define INS_FLOW_ACCEL_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(INS_FLOW_ACCEL_ID) + +/* default IMU lowpass to use in INS */ +#ifndef INS_FLOW_IMU_ID +#define INS_FLOW_IMU_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(INS_FLOW_IMU_ID) + + +/* default GPS to use in INS */ +#ifndef INS_FLOW_GPS_ID +#define INS_FLOW_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(INS_FLOW_GPS_ID) + +/* Use optical flow estimates */ +#ifndef INS_OPTICAL_FLOW_ID +#define INS_OPTICAL_FLOW_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(INS_OPTICAL_FLOW_ID) + +// reading RPMs: +#ifndef INS_RPM_ID +#define INS_RPM_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(INS_RPM_ID) + +/* All registered ABI events */ +static abi_event gyro_ev; +static abi_event accel_ev; +static abi_event gps_ev; +static abi_event ins_optical_flow_ev; +static abi_event ins_RPM_ev; +static abi_event aligner_ev; +//static abi_event mag_ev; +//static abi_event geo_mag_ev; + +/* All ABI callbacks */ +static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro); +static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel); +/*static void mag_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t __attribute__((unused)) stamp, + struct Int32Vect3 *mag); +static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h);*/ +//static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f); +static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s); +void ins_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, + int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence); +static void ins_rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act); +static void aligner_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t stamp __attribute__((unused)), + struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag); +static void print_ins_flow_state(void); +static void print_true_state(void); +/* Static local functions */ +//static bool ahrs_icq_output_enabled; +static uint32_t ahrs_icq_last_stamp; +static uint8_t ahrs_flow_id = AHRS_COMP_ID_FLOW; ///< Component ID for FLOW +static void set_body_state_from_quat(void); +static void ins_reset_filter(void); + +struct InsFlow { + + // data elements for gps passthrough: + struct LtpDef_i ltp_def; + bool ltp_initialized; + + /* output LTP NED */ + struct NedCoor_i ltp_pos; + struct NedCoor_i ltp_speed; + struct NedCoor_i ltp_accel; + + // vision measurements: + float optical_flow_x; + float optical_flow_y; + float divergence; + float vision_time; // perhaps better to use microseconds (us) instead of float in seconds + bool new_flow_measurement; + + // RPMs: + uint16_t RPM[8]; // max an octocopter + uint8_t RPM_num_act; + + float lp_gyro_pitch; + float lp_gyro_bias_pitch; // determine the bias before take-off + float lp_gyro_roll; + float lp_gyro_bias_roll; // determine the bias before take-off + float thrust_factor; // determine the additional required scale factor to have unbiased thrust estimates + float lp_thrust; + float lp_roll_command; + +}; +struct InsFlow ins_flow; + +// Kalman filter parameters and variables: + +#define MOMENT_DELAY 20 +float moments[MOMENT_DELAY] = {0.}; +int moment_ind; + +float OF_X[N_STATES_OF_KF] = {0.}; +float OF_Q[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}; +float OF_P[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}; +float OF_R[N_MEAS_OF_KF][N_MEAS_OF_KF] = {{0.}}; + +#define OF_N_ROTORS 4 +float RPM_FACTORS[OF_N_ROTORS]; + +float of_time; +float of_prev_time; +float lp_factor; +float lp_factor_strong; +bool reset_filter; +int use_filter; +bool run_filter; +uint32_t counter; +float thrust_factor; + +float GT_phi; +float GT_theta; + + +#define USE_STANDARD_PARAMS 0 + +#if USE_STANDARD_PARAMS +// Note that not all of the values are used. +// Moment of Inertia, mass, distance motors from center of gravity, 4 params for thrust and moment generation, +// measurement noise R (2), actuation noise Q(5), +// initial P (5), linear drag factor +#if USE_NPS +float parameters[20] = {0.0018244, 0.400, 0.085, 0.152163; 0.170734; 0.103436; 0.122109, + 0.1, 0.1, 0.1, 1.0f * M_PI / 180.0f, 100.0f * M_PI / 180.0f, 0.1f, 3.0f, + 1.0f, 10.0f * M_PI / 180.0f, 10.0f * M_PI / 180.0f, 1.0f, 1.0f, 0.5f + }; +#else +float parameters[20] = {0.0018244, 0.400, 0.085, 0.108068 0.115448 0.201207 0.208834, + 0.1, 0.1, 0.1, 1.0f * M_PI / 180.0f, 100.0f * M_PI / 180.0f, 0.1f, 3.0f, + 1.0f, 10.0f * M_PI / 180.0f, 10.0f * M_PI / 180.0f, 1.0f, 1.0f, 0.5f + }; +#endif +#else +// Define parameters for the filter, fitted in MATLAB: +#if USE_NPS +#if N_MEAS_OF_KF == 3 +// with rate measurement: +float parameters[20] = {1.234994e-01, 3.603662e-01, 8.751691e-02, 1.636867e-01, 1.561769e-01, 1.856140e-01, 1.601066e-02, 1.187989e-01, 1.507075e-01, 2.471644e-01, 7.934140e-02, 1.770048e+00, 1.345862e-01, 2.881410e+00, 1.003584e+00, 1.280523e-01, 7.549402e-02, 9.640423e-01, 1.078312e+00, 3.468849e-01}; +#else +// without rate state / measurement: +#if CONSTANT_ALT_FILTER +#if OF_DRAG +float parameters[20] = {1.396428e-01, 2.517970e-01, 3.575834e-02, 2.626194e-01, 1.078661e-01, 3.126137e-01, 4.621823e-02, 3.258048e-01, 8.456147e-02, 2.275105e-01, 2.820394e-02, 1.937395e+00, -4.259889e-02, 2.755648e+00, 1.000810e+00, -3.474577e-03, 3.146387e-01, 8.809383e-01, 9.878757e-01, 6.741976e-01}; +#else +float parameters[20] = {3.363769e-01, 4.917425e-01, 1.903805e-01, 2.945672e-01, 1.258647e-01, 1.513736e-01, 5.894541e-01, 2.162745e-01, 5.527361e-01, 1.385623e-01, 8.307731e-01, 1.488212e+00, 2.439721e-01, 3.052758e+00, 8.246426e-01, 9.988101e-02, 1.247046e-01, 8.834364e-01, 7.971876e-01, 1.112319e+00}; +#endif +#else +float parameters[20] = {4.370754e-02, 3.770587e-01, 1.187542e-01, 1.174995e-01, 1.419432e-01, 6.950201e-02, 2.251078e-01, 9.113943e-02, 2.230198e-01, 5.767389e-02, 1.855676e-02, 1.676359e+00, 5.822681e-02, 2.869468e+00, 1.140625e+00, 6.831383e-02, 1.600776e-01, 9.853843e-01, 1.000381e+00, 5.081224e-01}; +#endif +#endif +#else +// TODO: train constant alt filter without drag, also with and without measuring the gyro. +#if CONSTANT_ALT_FILTER +#if OF_DRAG +// float parameters[20] = {1.557784e-01, 3.186275e-01, 8.341852e-02, 9.320449e-02, 1.706694e-01, 3.950497e-01, 3.338107e-01, 1.947852e-01, 2.429782e-01, 1.216562e-01, 2.885142e-01, 1.765480e+00, 2.427392e-01, 3.014556e+00, 1.004227e+00, 1.798174e-01, 2.821081e-01, 9.314043e-01, 1.005090e+00, 2.630276e-01}; +float parameters[20] = {8.407886e-03, 4.056093e-01, 1.555919e-01, 1.291584e-01, 2.594766e-01, 1.927331e-01, 9.599609e-02, 1.688265e-01, 5.589618e-02, 1.605665e-01, 1.195912e-01, 1.809532e+00, 4.268251e-02, 3.003060e+00, 1.098473e+00, 1.944433e-01, 2.363352e-01, 1.110390e+00, 1.190994e+00, 6.211962e-01}; +#endif +#else +#if N_MEAS_OF_KF == 3 +#if PREDICT_GYROS == 0 +// with rate measurement (last two values copied from the last condition) +float parameters[22] = {0.041001, 1.015066, -0.058495, 0.498353, -0.156362, 0.383511, 0.924635, 0.681918, 0.318947, 0.298235, 0.224906, 1.371037, 0.008888, 3.045428, 0.893953, 0.529789, 0.295028, 1.297515, 0.767550, 0.334040, 0.192238, 0.301966}; +#else +// Estimate gyro directly instead of moment: +float parameters[26] = {1.065019, 0.270407, 0.164520, 0.008321, 0.083628, 0.261853, 0.210707, 0.204501, 0.164267, 0.097979, 0.053705, 1.640180, 0.151171, 3.086366, 1.025684, 0.011813, 0.177164, 0.995710, 1.050374, 0.617920, 0.028360, 0.447258, 0.077277, 0.360559, 0.555940, 0.133979}; +#endif +#else +// without rate measurement: +// float parameters[20] = {4.098677e-01, 7.766318e-01, 3.614751e-01, 4.745865e-01, 5.144065e-01, 3.113647e-01, -8.737287e-03, 6.370274e-01, 3.863760e-01, -3.527670e-01, 4.873666e-01, 1.688456e+00, -6.037967e-02, 2.759148e+00, 1.385455e+00, 1.044881e-01, -1.170409e-01, 1.126136e+00, 1.097562e+00, 2.680243e-01}; +float parameters[22] = {0.219033, 0.376572, 0.184002, 0.096388, 0.240843, 0.172390, 0.133111, 0.495885, 0.357086, 0.233624, 0.125611, 1.661682, 0.136735, 2.812652, 0.715887, 0.166932, 0.371409, 1.043920, 0.840683, 0.567703, 0.192238, 0.301966}; +#endif +#endif +#endif +#endif +// parameter indices (TODO: how important are these numbers? Some are not used, others, like P may be not so important). +#define PAR_IX 0 +#define PAR_MASS 1 +#define PAR_BASE 2 +#define PAR_K0 3 +#define PAR_K1 4 +#define PAR_K2 5 +#define PAR_K3 6 +#define PAR_R0 7 +#define PAR_R1 8 +#define PAR_Q0 9 +#define PAR_Q1 10 +#define PAR_Q2 11 +#define PAR_Q3 12 +#define PAR_Q4 13 +#define PAR_P0 14 +#define PAR_P1 15 +#define PAR_P2 16 +#define PAR_P3 17 +#define PAR_P4 18 +#define PAR_KD 19 +#define PAR_Q_TB 20 +#define PAR_P_TB 21 +#define PAR_PRED_ROLL_1 22 +#define PAR_PRED_ROLL_2 23 +#define PAR_PRED_ROLL_3 24 + + + +/* +struct InsFlowState { + // vector representation of state: + // v, angle, angle_dot, z, z_dot + + +}; +struct InsFlowState ins_flow_state; +*/ + +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" +#include "mcu_periph/sys_time.h" +#include "state.h" + +// attitude part: +static void send_quat(struct transport_tx *trans, struct link_device *dev) +{ + struct Int32Quat *quat = stateGetNedToBodyQuat_i(); + pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID, + &ahrs_icq.weight, + &ahrs_icq.ltp_to_body_quat.qi, + &ahrs_icq.ltp_to_body_quat.qx, + &ahrs_icq.ltp_to_body_quat.qy, + &ahrs_icq.ltp_to_body_quat.qz, + &(quat->qi), + &(quat->qx), + &(quat->qy), + &(quat->qz), + &ahrs_flow_id); +} + +static void send_euler(struct transport_tx *trans, struct link_device *dev) +{ + struct Int32Eulers ltp_to_imu_euler; + int32_eulers_of_quat(<p_to_imu_euler, &ahrs_icq.ltp_to_body_quat); + struct Int32Eulers *eulers = stateGetNedToBodyEulers_i(); + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, + <p_to_imu_euler.phi, + <p_to_imu_euler.theta, + <p_to_imu_euler.psi, + &(eulers->phi), + &(eulers->theta), + &(eulers->psi), + &ahrs_flow_id); + +} + +static void send_bias(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID, + &ahrs_icq.gyro_bias.p, &ahrs_icq.gyro_bias.q, + &ahrs_icq.gyro_bias.r, &ahrs_flow_id); +} + +static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) +{ + struct FloatVect3 h_float; + h_float.x = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.x); + h_float.y = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.y); + h_float.z = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.z); + pprz_msg_send_GEO_MAG(trans, dev, AC_ID, + &h_float.x, &h_float.y, &h_float.z, &ahrs_flow_id); +} + +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t mde = 3; + uint16_t val = 0; + if (!ahrs_icq.is_aligned) { mde = 2; } + uint32_t t_diff = get_sys_time_usec() - ahrs_icq_last_stamp; + /* set lost if no new gyro measurements for 50ms */ + if (t_diff > 50000) { mde = 5; } + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_flow_id, &mde, &val); +} + +// ins part +static void send_ins(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_INS(trans, dev, AC_ID, + &ins_flow.ltp_pos.x, &ins_flow.ltp_pos.y, &ins_flow.ltp_pos.z, + &ins_flow.ltp_speed.x, &ins_flow.ltp_speed.y, &ins_flow.ltp_speed.z, + &ins_flow.ltp_accel.x, &ins_flow.ltp_accel.y, &ins_flow.ltp_accel.z); +} + +/*static void send_ins_z(struct transport_tx *trans, struct link_device *dev) +{ + static float fake_baro_z = 0.0; + pprz_msg_send_INS_Z(trans, dev, AC_ID, + (float *)&fake_baro_z, &ins_flow.ltp_pos.z, + &ins_flow.ltp_speed.z, &ins_flow.ltp_accel.z); +}*/ + +static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) +{ + static float fake_qfe = 0.0; + if (ins_flow.ltp_initialized) { + pprz_msg_send_INS_REF(trans, dev, AC_ID, + &ins_flow.ltp_def.ecef.x, &ins_flow.ltp_def.ecef.y, &ins_flow.ltp_def.ecef.z, + &ins_flow.ltp_def.lla.lat, &ins_flow.ltp_def.lla.lon, &ins_flow.ltp_def.lla.alt, + &ins_flow.ltp_def.hmsl, (float *)&fake_qfe); + } +} + + +#endif + +static void send_ins_flow(struct transport_tx *trans, struct link_device *dev) +{ + // TODO: add sending of theta: + struct FloatEulers *eulers = stateGetNedToBodyEulers_f(); + struct NedCoor_f *position = stateGetPositionNed_f(); + struct NedCoor_f *velocities = stateGetSpeedNed_f(); + struct FloatRates *rates = stateGetBodyRates_f(); + + float phi = (180.0 / M_PI) * OF_X[OF_ANGLE_IND]; + float theta; + if (OF_TWO_DIM) { + theta = (180.0 / M_PI) * OF_X[OF_THETA_IND]; + } else { + // if not filtering the second dimension, just take the ground truth + theta = (180.0 / M_PI) * eulers->theta; + } + + float phi_dot = 0.0f; + float z_dot = 0.0f; + if (!CONSTANT_ALT_FILTER) { + phi_dot = (180.0 / M_PI) * OF_X[OF_ANGLE_DOT_IND]; + z_dot = OF_X[OF_Z_DOT_IND]; + } + + struct FloatRMat *NTB = stateGetNedToBodyRMat_f(); + struct FloatVect3 NED_velocities, body_velocities; + NED_velocities.x = velocities->x; + NED_velocities.y = velocities->y; + NED_velocities.z = velocities->z; + float_rmat_vmult(&body_velocities, NTB, &NED_velocities); + + float vy_GT = body_velocities.y; + + float phi_GT; + if (use_filter < USE_ANGLE) { + phi_GT = (180.0 / M_PI) * eulers->phi; + } else { + phi_GT = (180.0 / M_PI) * GT_phi; + } + float vx_GT = body_velocities.x; + float theta_GT; + if (use_filter < USE_ANGLE) { + theta_GT = (180.0 / M_PI) * eulers->theta; + } else if (OF_TWO_DIM) { + theta_GT = (180.0 / M_PI) * GT_theta; + } + float p_GT = rates->p; + float q_GT = rates->q; + float z_GT = -position->z; + float vz_GT = -velocities->z; + + float vy = OF_X[OF_V_IND]; + float vx; + if (OF_TWO_DIM) { + vx = OF_X[OF_VX_IND]; + } else { + vx = vx_GT; + } + float z = OF_X[OF_Z_IND]; + float p, q; + if (!CONSTANT_ALT_FILTER) { + // normally: + p = phi_dot; + // when estimating the gyros: + // // p = -1.8457e-04 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command); + // p = -2.0e-03 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command); + // TODO: expand the full filter later as well, to include q: + q = q_GT; + } else { + p = ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll; + // p = -2.0e-03 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command); + q = ins_flow.lp_gyro_pitch - ins_flow.lp_gyro_bias_pitch; + } + + float thrust_bias; + if (!OF_THRUST_BIAS || CONSTANT_ALT_FILTER) { + thrust_bias = 0.0f; + } else { + thrust_bias = OF_X[OF_THRUST_BIAS_IND]; + } + // This code is to actually compare the unbiased thrust with gravity: + // TODO: code copied from below, put in a function? + float thrust = 0.0f; + for (int i = 0; i < OF_N_ROTORS; i++) { + thrust += RPM_FACTORS[i] * ins_flow.RPM[i] * ins_flow.RPM[i]; + } + thrust *= thrust_factor; // ins_flow.thrust_factor; + thrust -= thrust_bias; + float mass = parameters[PAR_MASS]; + float g = 9.81; + float actual_lp_thrust = mass * g; + thrust_bias = thrust - actual_lp_thrust; + + pprz_msg_send_INS_FLOW_INFO(trans, dev, AC_ID, + &vy, &phi, &p, &vx, &theta, &q, &z, &z_dot, + &vy_GT, &phi_GT, &p_GT, &vx_GT, &theta_GT, &q_GT, + &z_GT, &vz_GT, &thrust_bias, &use_filter); +} + +void ins_reset_filter(void) +{ + + // (re-)initialize the state: + for (int i = 0; i < N_STATES_OF_KF; i++) { + OF_X[i] = 0.0f; + } + OF_X[OF_Z_IND] = 1.0; // nonzero z + + // P-matrix: + for (int i = 0; i < N_STATES_OF_KF; i++) { + for (int j = 0; j < N_STATES_OF_KF; j++) { + OF_P[i][j] = 0.0f; + } + } + if (CONSTANT_ALT_FILTER == 1) { + OF_P[OF_V_IND][OF_V_IND] = parameters[PAR_P0]; + OF_P[OF_ANGLE_IND][OF_ANGLE_IND] = parameters[PAR_P1]; + OF_P[OF_Z_IND][OF_Z_IND] = parameters[PAR_P3]; + OF_P[OF_ANGLE_DOT_IND][OF_ANGLE_DOT_IND] = parameters[PAR_P2]; + if (OF_TWO_DIM) { + OF_P[OF_THETA_IND][OF_THETA_IND] = parameters[PAR_P1]; + OF_P[OF_VX_IND][OF_VX_IND] = parameters[PAR_P0]; + } + } else { + OF_P[OF_V_IND][OF_V_IND] = parameters[PAR_P0]; + OF_P[OF_ANGLE_IND][OF_ANGLE_IND] = parameters[PAR_P1]; + OF_P[OF_ANGLE_DOT_IND][OF_ANGLE_DOT_IND] = parameters[PAR_P2]; + OF_P[OF_Z_IND][OF_Z_IND] = parameters[PAR_P3]; + OF_P[OF_Z_DOT_IND][OF_Z_DOT_IND] = parameters[PAR_P4]; + if (OF_THRUST_BIAS) { + OF_P[OF_THRUST_BIAS_IND][OF_THRUST_BIAS_IND] = parameters[PAR_P_TB];//OF_TB_P; + } + } + + counter = 0; + + // TODO: what to do with thrust, gyro bias, and low-passed roll command? + +} + + +/* Initialize the flow ins */ +void ins_flow_init(void) +{ + + //ahrs_icq_output_enabled = AHRS_ICQ_OUTPUT_ENABLED; + ahrs_icq_init(); + //ahrs_register_impl(ahrs_icq_enable_output); + + struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = NAV_LAT0; + llh_nav0.lon = NAV_LON0; + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = NAV_ALT0 + NAV_MSL0; + struct EcefCoor_i ecef_nav0; + ecef_of_lla_i(&ecef_nav0, &llh_nav0); + struct LtpDef_i ltp_def; + ltp_def_from_ecef_i(<p_def, &ecef_nav0); + + ltp_def_from_ecef_i(&ins_flow.ltp_def, &ecef_nav0); + ins_flow.ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_flow.ltp_def); + ins_flow.ltp_initialized = true; + ins_flow.new_flow_measurement = false; + ins_flow.lp_gyro_pitch = 0.0f; + ins_flow.lp_gyro_bias_pitch = 0.0f; + ins_flow.lp_gyro_roll = 0.0f; + ins_flow.lp_gyro_bias_roll = 0.0f; + ins_flow.thrust_factor = 1.0f; + ins_flow.lp_thrust = 0.0f; + ins_flow.lp_roll_command = 0.0f; + + lp_factor = 0.95; + lp_factor_strong = 1 - 1E-3; + + GT_phi = 0.0f; + GT_theta = 0.0f; + + // Extended Kalman filter: + // reset the state and P matrix: + ins_reset_filter(); + + // R-matrix, measurement noise (TODO: make params) + OF_R[OF_LAT_FLOW_IND][OF_LAT_FLOW_IND] = parameters[PAR_R0]; + OF_R[OF_DIV_FLOW_IND][OF_DIV_FLOW_IND] = parameters[PAR_R1]; + if (OF_TWO_DIM) { + OF_R[OF_LAT_FLOW_X_IND][OF_LAT_FLOW_X_IND] = parameters[PAR_R0]; + } else if (N_MEAS_OF_KF == 3) { + OF_R[OF_RATE_IND][OF_RATE_IND] = 1.0 * (M_PI / 180.0f); // not a param yet, used to be 10, but we could trust it more + } + // Q-matrix, actuation noise (TODO: make params) + OF_Q[OF_V_IND][OF_V_IND] = parameters[PAR_Q0]; + OF_Q[OF_ANGLE_IND][OF_ANGLE_IND] = parameters[PAR_Q1]; + OF_Q[OF_Z_IND][OF_Z_IND] = parameters[PAR_Q3]; + OF_Q[OF_ANGLE_DOT_IND][OF_ANGLE_DOT_IND] = parameters[PAR_Q2]; + if (!CONSTANT_ALT_FILTER) { + OF_Q[OF_Z_DOT_IND][OF_Z_DOT_IND] = parameters[PAR_Q4]; + } else if (OF_TWO_DIM) { + OF_Q[OF_VX_IND][OF_VX_IND] = parameters[PAR_Q0]; + OF_Q[OF_THETA_IND][OF_THETA_IND] = parameters[PAR_Q1]; + } + if (OF_THRUST_BIAS) { + OF_Q[OF_THRUST_BIAS_IND][OF_THRUST_BIAS_IND] = parameters[PAR_Q_TB];//OF_TB_Q; + } + + + // based on a fit, factor * rpm^2: + // TODO: with the parameters, we don't need this if / else any more. +#if USE_NPS + // K = [0.152163; 0.170734; 0.103436; 0.122109] * 1E-7; + // K = [0.222949; 0.160458; 0.114227; 0.051396] * 1E-7; + // rpm: + // [2708.807954; 2587.641476; -379.728916; -501.203388] + RPM_FACTORS[0] = parameters[PAR_K0] * 1E-7; + RPM_FACTORS[1] = parameters[PAR_K1] * 1E-7; + RPM_FACTORS[2] = parameters[PAR_K2] * 1E-7; + RPM_FACTORS[3] = parameters[PAR_K3] * 1E-7; +#else + // % Bebop 2, #45 + // From fit_TM_2 script: + // K = [0.108068; 0.115448; 0.201207; 0.208834] * 1E-7 + RPM_FACTORS[0] = parameters[PAR_K0] * 1E-7; + RPM_FACTORS[1] = parameters[PAR_K1] * 1E-7; + RPM_FACTORS[2] = parameters[PAR_K2] * 1E-7; + RPM_FACTORS[3] = parameters[PAR_K3] * 1E-7; +#endif + + reset_filter = false; + use_filter = 0; + run_filter = false; + + of_time = get_sys_time_float(); + of_prev_time = get_sys_time_float(); + + // align the AHRS: + ahrs_aligner_init(); + // set the initial attitude: + set_body_state_from_quat(); + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_QUAT_INT, send_quat); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_euler); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_GYRO_BIAS_INT, send_bias); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GEO_MAG, send_geo_mag); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS, send_ins); + //register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref); +#endif + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ +// TODO: warning: passing argument 3 of ‘AbiBindMsgIMU_GYRO_INT’ from incompatible pointer type + AbiBindMsgIMU_GYRO(INS_FLOW_GYRO_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL(INS_FLOW_ACCEL_ID, &accel_ev, accel_cb); + AbiBindMsgGPS(INS_FLOW_GPS_ID, &gps_ev, gps_cb); + //AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); + AbiBindMsgOPTICAL_FLOW(INS_OPTICAL_FLOW_ID, &ins_optical_flow_ev, ins_optical_flow_cb); + AbiBindMsgRPM(INS_RPM_ID, &ins_RPM_ev, ins_rpm_cb); + AbiBindMsgIMU_LOWPASSED(INS_FLOW_IMU_ID, &aligner_ev, aligner_cb); + // AbiBindMsgIMU_MAG_INT32(ABI_BROADCAST, &mag_ev, mag_cb); + // AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); + + // Telemetry: + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_FLOW_INFO, send_ins_flow); + + moment_ind = 0; + +} + +void ins_reset_local_origin(void) +{ + ltp_def_from_ecef_i(&ins_flow.ltp_def, &gps.ecef_pos); + ins_flow.ltp_def.lla.alt = gps.lla_pos.alt; + ins_flow.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_flow.ltp_def); + ins_flow.ltp_initialized = true; +} + +void ins_optical_flow_cb(uint8_t sender_id UNUSED, uint32_t stamp, int32_t flow_x UNUSED, + int32_t flow_y UNUSED, + int32_t flow_der_x, int32_t flow_der_y, float quality UNUSED, float size_divergence) +{ + + // TODO: make parameters: + float subpixel_factor = 10.0f; + float focal_x = 347.22; + float new_time = ((float)stamp) / 1e6; + float fps = 1.0f / (new_time - ins_flow.vision_time); + ins_flow.optical_flow_x = (((float)flow_x) * fps) / (subpixel_factor * focal_x); + ins_flow.optical_flow_y = (((float)flow_y) * fps) / (subpixel_factor * focal_x); + ins_flow.divergence = 1.27 * size_divergence * fps; + //printf("Reading %f, %f, %f\n", ins_flow.optical_flow_x, ins_flow.optical_flow_y, ins_flow.divergence); + ins_flow.vision_time = new_time; + ins_flow.new_flow_measurement = true; + +} + +void print_ins_flow_state(void) +{ + if (CONSTANT_ALT_FILTER) { + if (!OF_TWO_DIM) { + printf("v = %f, angle = %f, angle_dot = %f, z = %f.\n", + OF_X[OF_V_IND], OF_X[OF_ANGLE_IND], OF_X[OF_ANGLE_DOT_IND], OF_X[OF_Z_IND]); + } else { + printf("v = %f, angle = %f, angle_dot = %f, z = %f, vx = %f, theta = %f.\n", + OF_X[OF_V_IND], OF_X[OF_ANGLE_IND], OF_X[OF_ANGLE_DOT_IND], OF_X[OF_Z_IND], OF_X[OF_VX_IND], OF_X[OF_THETA_IND]); + } + + } else { + if (!OF_THRUST_BIAS) { + printf("v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f.\n", + OF_X[OF_V_IND], OF_X[OF_ANGLE_IND], OF_X[OF_ANGLE_DOT_IND], OF_X[OF_Z_IND], OF_X[OF_Z_DOT_IND]); + } else { + printf("v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f, thrust bias = %f.\n", + OF_X[OF_V_IND], OF_X[OF_ANGLE_IND], OF_X[OF_ANGLE_DOT_IND], OF_X[OF_Z_IND], OF_X[OF_Z_DOT_IND], + OF_X[OF_THRUST_BIAS_IND]); + } + + } + +} + +void print_true_state(void) +{ + // TODO: rotate velocities to body frame: + // TODO: add also the theta axis: + struct FloatEulers *eulers = stateGetNedToBodyEulers_f(); + struct NedCoor_f *position = stateGetPositionNed_f(); + struct NedCoor_f *velocities = stateGetSpeedNed_f(); + struct FloatRates *rates = stateGetBodyRates_f(); + + printf("True: v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f.\n", + velocities->y, eulers->phi, rates->p, -position->z, -velocities->z); +} + +void ins_flow_update(void) +{ + float mass = parameters[PAR_MASS]; // 0.400; + float moment = 0.0f; // for now assumed to be 0 + float Ix = parameters[PAR_IX]; // 0.0018244; + //float b = parameters[PAR_BASE]; + float g = 9.81; // TODO: get a more accurate definition from pprz + float kd = parameters[PAR_KD]; // 0.5 + float drag = 0.0f; + + if (reset_filter) { + ins_reset_filter(); + reset_filter = false; + } + + // get ground truth data: + //struct FloatEulers* eulers = stateGetNedToBodyEulers_f(); + //struct NedCoor_f* position = stateGetPositionNed_f(); + //struct NedCoor_f *velocities = stateGetSpeedNed_f(); + //struct FloatRates *rates = stateGetBodyRates_f(); + + // TODO: record when starting from the ground: does that screw up the filter? Yes it does : ) + + + // assuming that the typical case is no rotation, we can estimate the (initial) bias of the gyro: + ins_flow.lp_gyro_bias_roll = lp_factor_strong * ins_flow.lp_gyro_bias_roll + (1 - lp_factor_strong) * + ins_flow.lp_gyro_roll; + if (OF_TWO_DIM) { + ins_flow.lp_gyro_bias_pitch = lp_factor_strong * ins_flow.lp_gyro_bias_pitch + (1 - lp_factor_strong) * + ins_flow.lp_gyro_pitch; + } + // same assumption for the roll command: assuming a close-to-hover situation and roll trim for staying in place: + ins_flow.lp_roll_command = lp_factor_strong * ins_flow.lp_roll_command + (1 - lp_factor_strong) * + stabilization_cmd[COMMAND_ROLL]; + + // only start estimation when flying (and above 1 meter: || position->z > -1.0f ) + // I removed the condition on height, since (1) we need to start the filter anyway explicitly now, and (2) it created a dependence on GPS fix. + if (!autopilot_in_flight()) { + return; + } + + if (!run_filter) { + + // TODO: should we do this if we have a thrust bias? + + // Drone is flying but not yet running the filter, in order to obtain unbiased thrust estimates we estimate an additional factor. + // Assumption is that the drone is hovering, which means that over a longer period of time, the thrust should equal gravity. + // If the low pass thrust is lower than the one expected by gravity, then it needs to be increased and viceversa. + float thrust = 0.0f; + for (int i = 0; i < OF_N_ROTORS; i++) { + thrust += RPM_FACTORS[i] * ins_flow.RPM[i] * ins_flow.RPM[i]; + } + if (ins_flow.lp_thrust < 1E-3) { + // first time directly initialize with thrust to get quicker convergence: + ins_flow.lp_thrust = thrust; + } else { + ins_flow.lp_thrust = lp_factor_strong * ins_flow.lp_thrust + (1 - lp_factor_strong) * thrust; + } + float actual_lp_thrust = mass * g; + ins_flow.thrust_factor = actual_lp_thrust / ins_flow.lp_thrust; + thrust_factor = ins_flow.thrust_factor; + //printf("Low pass predicted thrust = %f. Expected thrust = %f. Thrust factor = %f.\n", ins_flow.lp_thrust, actual_lp_thrust, ins_flow.thrust_factor); + // don't run the filter just yet: + return; + } + + if (DEBUG_INS_FLOW) { print_true_state(); } + + // in the sim, the gyro bias wanders so fast, that this does not seem to be useful: + // TODO: verify how this is in reality, and if not useful, remove all code to estimate this bias (or do it differently) + // ins_flow.lp_gyro_bias_roll = 0.0f; + + // This module needs to run at the autopilot speed when not yet using this filter. Plus it needs to estimate thrust and gyro bias at that speed. + // However, the updates of the filter themselves should be slower: + /*counter++; + if(counter < 5) { + return; + } + else { + counter = 0; + }*/ + + // get the new time: + of_time = get_sys_time_float(); + float dt = of_time - of_prev_time; + //printf("dt = %f.\n", dt); + if (dt > 1.0f) { + dt = 0.01f; + } + + // predict the thrust and moment: + float thrust = 0.0f; + for (int i = 0; i < OF_N_ROTORS; i++) { + thrust += RPM_FACTORS[i] * ins_flow.RPM[i] * ins_flow.RPM[i]; + } + thrust *= thrust_factor; // ins_flow.thrust_factor; + if (OF_THRUST_BIAS && !CONSTANT_ALT_FILTER) { + thrust -= OF_X[OF_THRUST_BIAS_IND]; + } + DEBUG_PRINT("Thrust acceleration = %f, g = %f\n", thrust / mass, g); + + // TODO: do we have an optimization that used the moment? + /*moment = b * RPM_FACTORS[0] * ins_flow.RPM[0]*ins_flow.RPM[0] - + b * RPM_FACTORS[1] * ins_flow.RPM[1]*ins_flow.RPM[1] - + b * RPM_FACTORS[2] * ins_flow.RPM[2]*ins_flow.RPM[2] + + b * RPM_FACTORS[3] * ins_flow.RPM[3]*ins_flow.RPM[3];*/ + // M_est = Ix * (-0.000553060716181365 * cmd_roll(k) -3.23315441805895 * Xe(3, k)); +#if USE_NPS + // TODO: moment in simulation is very easy to estimate with the roll command, so add that: + moment = 0; +#else + + /* + moments[moment_ind] = Ix *(-0.000553060716181365 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command) -3.23315441805895 * OF_X[OF_ANGLE_DOT_IND]); + + int select_ind = moment_ind - MOMENT_DELAY; + if(select_ind < 0) { + select_ind += MOMENT_DELAY; + } + + // current moment is a delayed version: + moment = moments[select_ind]; + + // update the moment's ind: + moment_ind++; + if(moment_ind >= MOMENT_DELAY) { + moment_ind = 0; + } + */ + // moment = Ix *(-0.000553060716181365 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command) -3.23315441805895 * OF_X[OF_ANGLE_DOT_IND]); + moment = 0; +#endif + + // printf("Predicted moment = %f, gyro = %f\n", moment, dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f); + + + // propagate the state with Euler integration: + DEBUG_PRINT("Before prediction: "); + if (DEBUG_INS_FLOW) { print_ins_flow_state(); } + if (CONSTANT_ALT_FILTER) { + OF_X[OF_V_IND] += dt * (g * tan(OF_X[OF_ANGLE_IND])); + if (OF_DRAG) { + // quadratic drag acceleration: + drag = dt * kd * (OF_X[OF_V_IND] * OF_X[OF_V_IND]) / mass; + // apply it in the right direction: + if (OF_X[OF_V_IND] > 0) { OF_X[OF_V_IND] -= drag; } + else { OF_X[OF_V_IND] += drag; } + } + + /* // if we use gyros here, the angle dot estimate is ignored: + * if(OF_USE_GYROS) { + // OF_X[OF_ANGLE_IND] += dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f; // Code says scaled by 12, but... that does not fit... + } */ + + // temporary insertion of gyro estimate here, for quicker effect: + // OF_X[OF_ANGLE_IND] += dt * -2.0e-03 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command); + + OF_X[OF_ANGLE_IND] += dt * OF_X[OF_ANGLE_DOT_IND]; + OF_X[OF_ANGLE_DOT_IND] += dt * (moment / Ix); + + if (OF_TWO_DIM) { + // Second axis, decoupled formulation: + OF_X[OF_VX_IND] += dt * (g * tan(OF_X[OF_THETA_IND])); + if (OF_DRAG) { + // quadratic drag acceleration: + drag = dt * kd * (OF_X[OF_VX_IND] * OF_X[OF_VX_IND]) / mass; + // apply it in the right direction: + if (OF_X[OF_VX_IND] > 0) { OF_X[OF_VX_IND] -= drag; } + else { OF_X[OF_VX_IND] += drag; } + } + // TODO: here also a moment estimate? + // TODO: add a THETA_DOT_IND + OF_X[OF_THETA_IND] += dt * (ins_flow.lp_gyro_pitch - ins_flow.lp_gyro_bias_pitch) * + (M_PI / 180.0f) / 74.0f; // Code says scaled by 12, but... that does not fit... + } + + DEBUG_PRINT("Rate p = %f, gyro p = %f\n", rates->p, + (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI / 180.0f) / 74.0f); + } else { + // make sure that the right hand state terms appear before they change: + OF_X[OF_V_IND] += dt * (thrust * sinf(OF_X[OF_ANGLE_IND]) / mass); + OF_X[OF_Z_IND] += dt * OF_X[OF_Z_DOT_IND]; + OF_X[OF_Z_DOT_IND] += dt * (thrust * cosf(OF_X[OF_ANGLE_IND]) / mass - g); + OF_X[OF_ANGLE_IND] += dt * OF_X[OF_ANGLE_DOT_IND]; + /* + * // TODO: We now only keep this here because it worked on the real drone. It also worked without it. So to be deleted if it works as is. + else { + OF_X[OF_ANGLE_IND] += dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f; + }*/ + OF_X[OF_ANGLE_DOT_IND] += dt * (moment / Ix); + + // thrust bias does not change over time according to our model + + if (OF_DRAG) { + // quadratic drag acceleration: + drag = dt * kd * (OF_X[OF_V_IND] * OF_X[OF_V_IND]) / mass; + // apply it in the right direction: + if (OF_X[OF_V_IND] > 0) { OF_X[OF_V_IND] -= drag; } + else { OF_X[OF_V_IND] += drag; } + } + } + + // ensure that z is not 0 (or lower) + if (OF_X[OF_Z_IND] < 1e-2) { + OF_X[OF_Z_IND] = 1e-2; + } + + DEBUG_PRINT("After prediction: "); + if (DEBUG_INS_FLOW) { print_ins_flow_state(); } + + // prepare the update and correction step: + // we have to recompute these all the time, as they depend on the state: + // discrete version of state transition matrix F: (ignoring t^2) + float F[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}; + for (int i = 0; i < N_STATES_OF_KF; i++) { + F[i][i] = 1.0f; + } + + if(CONSTANT_ALT_FILTER) { + F[OF_V_IND][OF_ANGLE_IND] = dt*(g/(cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND]))); + F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt*1.0f; + if(OF_TWO_DIM) { + F[OF_VX_IND][OF_THETA_IND] = dt*(g/(cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND]))); + } + } + else { + F[OF_V_IND][OF_ANGLE_IND] = dt*(thrust*cosf(OF_X[OF_ANGLE_IND])/mass); + F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt*1.0f; + F[OF_Z_IND][OF_Z_DOT_IND] = dt*1.0f; + F[OF_Z_DOT_IND][OF_ANGLE_IND] = dt*(-thrust*sinf(OF_X[OF_ANGLE_IND])/mass); + if(OF_THRUST_BIAS) { + F[OF_V_IND][OF_THRUST_BIAS_IND] = -dt*sinf(OF_X[OF_ANGLE_IND]) / mass; + F[OF_Z_DOT_IND][OF_THRUST_BIAS_IND] = -dt*cosf(OF_X[OF_ANGLE_IND]) / mass; + } + } + + // G matrix (whatever it may be): + float G[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}; + // TODO: we miss an off-diagonal element here (compare with MATLAB) + for (int i = 0; i < N_STATES_OF_KF; i++) { + G[i][i] = dt; + } + + // Jacobian observation matrix H: + float H[N_MEAS_OF_KF][N_STATES_OF_KF] = {{0.}}; + + if(CONSTANT_ALT_FILTER) { + // Hx = [-cosf(theta)^2/z, (v*sinf(theta))/ z, (v* cosf(theta)^2)/z^2]; + // lateral flow: + H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/ OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]); + H[OF_LAT_FLOW_IND][OF_ANGLE_DOT_IND] = 1.0f; + // divergence: + H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); + H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; + H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND]); + H[OF_DIV_FLOW_IND][OF_ANGLE_DOT_IND] = 0.0f; + + if(OF_TWO_DIM) { + // divergence measurement couples the two axes actually...: + H[OF_DIV_FLOW_IND][OF_VX_IND] = -sinf(2*OF_X[OF_THETA_IND])/(2*OF_X[OF_Z_IND]); + H[OF_DIV_FLOW_IND][OF_THETA_IND] = -OF_X[OF_VX_IND]*cosf(2*OF_X[OF_THETA_IND])/OF_X[OF_Z_IND]; + + // lateral flow in x direction: + H[OF_LAT_FLOW_X_IND][OF_VX_IND] = -cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/ OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_X_IND][OF_THETA_IND] = OF_X[OF_VX_IND]*sinf(2*OF_X[OF_THETA_IND])/OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_X_IND][OF_Z_IND] = OF_X[OF_VX_IND]*cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]); + } + } else { + // lateral flow: + H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/ OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] + + OF_X[OF_Z_DOT_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_IND][OF_ANGLE_DOT_IND] = 1.0f; + H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]) + - OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND]); + H[OF_LAT_FLOW_IND][OF_Z_DOT_IND] = sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); + // divergence: + H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); + H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] + + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]; + H[OF_DIV_FLOW_IND][OF_ANGLE_DOT_IND] = 0.0f; + H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND]) + + OF_X[OF_Z_DOT_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]); + H[OF_DIV_FLOW_IND][OF_Z_DOT_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; + } + + // rate measurement: + if (OF_USE_GYROS) { + H[OF_RATE_IND][OF_V_IND] = 0.0f; + H[OF_RATE_IND][OF_ANGLE_IND] = 0.0f; + H[OF_RATE_IND][OF_ANGLE_DOT_IND] = 1.0f; + H[OF_RATE_IND][OF_Z_IND] = 0.0f; + H[OF_RATE_IND][OF_Z_DOT_IND] = 0.0f; + } + + // propagate uncertainty: + // TODO: make pointers that don't change to init: + MAKE_MATRIX_PTR(Phi, F, N_STATES_OF_KF); + MAKE_MATRIX_PTR(P, OF_P, N_STATES_OF_KF); + MAKE_MATRIX_PTR(Gamma, G, N_STATES_OF_KF); + MAKE_MATRIX_PTR(Q, OF_Q, N_STATES_OF_KF); + MAKE_MATRIX_PTR(R, OF_R, N_MEAS_OF_KF); + MAKE_MATRIX_PTR(Jac, H, N_MEAS_OF_KF); + + DEBUG_PRINT("Phi:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, Phi); + + DEBUG_PRINT("P:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, P); + + DEBUG_PRINT("Gamma:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, G); + + DEBUG_PRINT("Q:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, Q); + + DEBUG_PRINT("R:\n"); + DEBUG_MAT_PRINT(N_MEAS_OF_KF, N_MEAS_OF_KF, R); + + DEBUG_PRINT("Jacobian:\n"); + DEBUG_MAT_PRINT(N_MEAS_OF_KF, N_STATES_OF_KF, Jac); + + // Corresponding MATLAB statement: :O + // P_k1_k = Phi_k1_k*P*Phi_k1_k' + Gamma_k1_k*Q*Gamma_k1_k'; + float _PhiT[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(PhiT, _PhiT, N_STATES_OF_KF); + float _P_PhiT[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(PPhiT, _P_PhiT, N_STATES_OF_KF); + float _Phi_P_PhiT[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(PhiPPhiT, _Phi_P_PhiT, N_STATES_OF_KF); + + float_mat_transpose(PhiT, Phi, N_STATES_OF_KF, N_STATES_OF_KF); + float_mat_mul(PPhiT, P, PhiT, N_STATES_OF_KF, N_STATES_OF_KF, N_STATES_OF_KF); + float_mat_mul(PhiPPhiT, Phi, PPhiT, N_STATES_OF_KF, N_STATES_OF_KF, N_STATES_OF_KF); + + DEBUG_PRINT("Phi*P*PhiT:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, PhiPPhiT); + + float _GT[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(GT, _GT, N_STATES_OF_KF); + float _Q_GT[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(QGT, _Q_GT, N_STATES_OF_KF); + float _G_Q_GT[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(GQGT, _G_Q_GT, N_STATES_OF_KF); + + float_mat_transpose(GT, Gamma, N_STATES_OF_KF, N_STATES_OF_KF); + float_mat_mul(QGT, Q, GT, N_STATES_OF_KF, N_STATES_OF_KF, N_STATES_OF_KF); + float_mat_mul(GQGT, Gamma, QGT, N_STATES_OF_KF, N_STATES_OF_KF, N_STATES_OF_KF); + + DEBUG_PRINT("Gamma*Q*GammaT:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, GQGT); + + float_mat_sum(P, PhiPPhiT, GQGT, N_STATES_OF_KF, N_STATES_OF_KF); + DEBUG_PRINT("P:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, P); + + // correct state when there is a new vision measurement: + if (ins_flow.new_flow_measurement) { + + DEBUG_PRINT("*********************\n"); + DEBUG_PRINT(" NEW MEASUREMENT \n"); + DEBUG_PRINT("*********************\n"); + + // determine Kalman gain: + // MATLAB statement: + // S_k = Hx*P_k1_k*Hx' + R; + float _JacT[N_STATES_OF_KF][N_MEAS_OF_KF]; + MAKE_MATRIX_PTR(JacT, _JacT, N_STATES_OF_KF); + float _P_JacT[N_STATES_OF_KF][N_MEAS_OF_KF]; + MAKE_MATRIX_PTR(PJacT, _P_JacT, N_STATES_OF_KF); + float _Jac_P_JacT[N_MEAS_OF_KF][N_MEAS_OF_KF]; + MAKE_MATRIX_PTR(JacPJacT, _Jac_P_JacT, N_MEAS_OF_KF); + + float_mat_transpose(JacT, Jac, N_MEAS_OF_KF, N_STATES_OF_KF); + float_mat_mul(PJacT, P, JacT, N_STATES_OF_KF, N_STATES_OF_KF, N_MEAS_OF_KF); + DEBUG_PRINT("P*JacT:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_MEAS_OF_KF, PJacT); + + float_mat_mul(JacPJacT, Jac, PJacT, N_MEAS_OF_KF, N_STATES_OF_KF, N_MEAS_OF_KF); + + DEBUG_PRINT("Jac*P*JacT:\n"); + DEBUG_MAT_PRINT(N_MEAS_OF_KF, N_MEAS_OF_KF, JacPJacT); + + float _S[N_MEAS_OF_KF][N_MEAS_OF_KF]; + MAKE_MATRIX_PTR(S, _S, N_MEAS_OF_KF); + float_mat_sum(S, JacPJacT, R, N_MEAS_OF_KF, N_MEAS_OF_KF); + + DEBUG_PRINT("S:\n"); + DEBUG_MAT_PRINT(N_MEAS_OF_KF, N_MEAS_OF_KF, S); + + // MATLAB statement: + // K_k1 = P_k1_k*Hx' * inv(S_k); + float _K[N_STATES_OF_KF][N_MEAS_OF_KF]; + MAKE_MATRIX_PTR(K, _K, N_STATES_OF_KF); + float _INVS[N_MEAS_OF_KF][N_MEAS_OF_KF]; + MAKE_MATRIX_PTR(INVS, _INVS, N_MEAS_OF_KF); + float_mat_invert(INVS, S, N_MEAS_OF_KF); + if (DEBUG_INS_FLOW) { + // This should be the identity matrix: + float _SINVS[N_MEAS_OF_KF][N_MEAS_OF_KF]; + MAKE_MATRIX_PTR(SINVS, _SINVS, N_MEAS_OF_KF); + float_mat_mul(SINVS, S, INVS, N_MEAS_OF_KF, N_MEAS_OF_KF, N_MEAS_OF_KF); + DEBUG_PRINT("S*Inv(S):\n"); + DEBUG_MAT_PRINT(N_MEAS_OF_KF, N_MEAS_OF_KF, SINVS); + } + + float_mat_mul(K, PJacT, INVS, N_STATES_OF_KF, N_MEAS_OF_KF, N_MEAS_OF_KF); + DEBUG_PRINT("K:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_MEAS_OF_KF, K); + + // Correct the state: + // MATLAB: + // Z_expected = [-v*cosf(theta)*cosf(theta)/z + zd*sinf(2*theta)/(2*z) + thetad; + // (-v*sinf(2*theta)/(2*z)) - zd*cosf(theta)*cosf(theta)/z]; + + float Z_expected[N_MEAS_OF_KF]; + + // TODO: take this var out? It was meant for debugging... + //float Z_expect_GT_angle; + + if(CONSTANT_ALT_FILTER) { + Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] + +OF_X[OF_ANGLE_DOT_IND]; // TODO: Currently, no p works better than using p here. Analyze! + + + /* TODO: remove later, just for debugging: + float Z_exp_no_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; + float Z_exp_with_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]+OF_X[OF_ANGLE_DOT_IND]; + printf("Z_exp_no_rate = %f, Z_exp_with_rate = %f, measured = %f, angle dot = %f, p = %f: ", Z_exp_no_rate, Z_exp_with_rate, + ins_flow.optical_flow_x, OF_X[OF_ANGLE_DOT_IND], dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f); + if(fabs(ins_flow.optical_flow_x - Z_exp_no_rate) < fabs(ins_flow.optical_flow_x - Z_exp_with_rate)) { + printf("NO RATE WINS!"); + } + printf("\n");*/ + + Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); + + if(OF_TWO_DIM) { + Z_expected[OF_LAT_FLOW_X_IND] = -OF_X[OF_VX_IND]*cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/OF_X[OF_Z_IND]; // TODO: no q? + } + + //Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND]; + + if (OF_USE_GYROS) { + Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction? + } + } + else { + Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] + + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]) + + OF_X[OF_ANGLE_DOT_IND]; // TODO: We first had this rate term but not for the constant alt filter. + // Simulation and data analysis from real flights shows that including it is better. CHECK IN REALITY! + + Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]) + -OF_X[OF_Z_DOT_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; + + //Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND] + // + OF_X[OF_Z_DOT_IND]*sinf(2*eulers->phi)/(2*OF_X[OF_Z_IND]); + //+ OF_X[OF_ANGLE_DOT_IND]; + if(N_MEAS_OF_KF == 3) { + Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction? + } + +/* + float Z_exp_no_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] + + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); + float Z_exp_with_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] + + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]) + + OF_X[OF_ANGLE_DOT_IND]; +*/ + /* + printf("Z_exp_no_rate = %f, Z_exp_with_rate = %f, measured = %f, angle dot = %f, p = %f: ", Z_exp_no_rate, Z_exp_with_rate, + ins_flow.optical_flow_x, OF_X[OF_ANGLE_DOT_IND], dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f); + if(fabs(ins_flow.optical_flow_x - Z_exp_no_rate) < fabs(ins_flow.optical_flow_x - Z_exp_with_rate)) { + printf("NO RATE WINS!"); + } + printf("\n");*/ + } + + // i_k1 = Z - Z_expected; + float innovation[N_MEAS_OF_KF][1]; + //print_ins_flow_state(); + innovation[OF_LAT_FLOW_IND][0] = ins_flow.optical_flow_x - Z_expected[OF_LAT_FLOW_IND]; + DEBUG_PRINT("Expected flow filter: %f, Expected flow ground truth = %f, Real flow x: %f, Real flow y: %f.\n", + Z_expected[OF_LAT_FLOW_IND], Z_expect_GT_angle, ins_flow.optical_flow_x, ins_flow.optical_flow_y); + innovation[OF_DIV_FLOW_IND][0] = ins_flow.divergence - Z_expected[OF_DIV_FLOW_IND]; + DEBUG_PRINT("Expected div: %f, Real div: %f.\n", Z_expected[OF_DIV_FLOW_IND], ins_flow.divergence); + if (CONSTANT_ALT_FILTER && OF_TWO_DIM) { + innovation[OF_LAT_FLOW_X_IND][0] = ins_flow.optical_flow_y - Z_expected[OF_LAT_FLOW_X_IND]; + DEBUG_PRINT("Expected flow in body X direction filter: %f, Real flow in corresponding y direction: %f, gyro = %f, expected velocity = %f, real velocity = %f, expected theta = %f, real theta = %f.\n", + Z_expected[OF_LAT_FLOW_X_IND], ins_flow.optical_flow_y, ins_flow.lp_gyro_pitch - ins_flow.lp_gyro_bias_pitch, + OF_X[OF_VX_IND], velocities->x, OF_X[OF_THETA_IND], eulers->theta); + } + if (OF_USE_GYROS) { + float gyro_meas_roll; + if (!PREDICT_GYROS) { + gyro_meas_roll = (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI / 180.0f) / 74.0f; + } else { + // TODO: You can fake gyros here by estimating them as follows: + // rate_p_filt_est = -1.8457e-04 * cmd_roll; + // gyro_meas_roll = -1.8457e-04 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command); + // gyro_meas_roll = -2.0e-03 * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command); + + // gyro_meas_roll = 1e-04 * parameters[PAR_PRED_ROLL_1] * (stabilization_cmd[COMMAND_ROLL]-ins_flow.lp_roll_command); + // gyro_meas_roll = parameters[PAR_PRED_ROLL_2] * gyro_meas_roll + 1E-3 * parameters[PAR_PRED_ROLL_3] * ins_flow.optical_flow_x; + + // only flow: + gyro_meas_roll = 2E-3 * parameters[PAR_PRED_ROLL_3] * ins_flow.optical_flow_x; + + //printf("Predicted roll: %f, real measured roll: %f.\n", gyro_meas_roll, (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f); + } + + innovation[OF_RATE_IND][0] = gyro_meas_roll - Z_expected[OF_RATE_IND]; + //innovation[OF_RATE_IND][0] = rates->p - Z_expected[OF_RATE_IND]; + DEBUG_PRINT("Expected rate: %f, Real rate: %f.\n", Z_expected[OF_RATE_IND], ins_flow.lp_gyro_roll); + } + + MAKE_MATRIX_PTR(I, innovation, N_MEAS_OF_KF); + DEBUG_PRINT("Innovation:"); + DEBUG_MAT_PRINT(N_MEAS_OF_KF, 1, I); + + // X_k1_k1 = X_k1_k + K_k1*(i_k1); + float _KI[N_STATES_OF_KF][1]; + MAKE_MATRIX_PTR(KI, _KI, N_STATES_OF_KF); + float_mat_mul(KI, K, I, N_STATES_OF_KF, N_MEAS_OF_KF, 1); + + DEBUG_PRINT("K*innovation:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, 1, KI); + + DEBUG_PRINT("PRE: v = %f, angle = %f\n", OF_X[OF_V_IND], OF_X[OF_ANGLE_IND]); + for (int i = 0; i < N_STATES_OF_KF; i++) { + OF_X[i] += KI[i][0]; + } + DEBUG_PRINT("POST v: %f, angle = %f\n", OF_X[OF_V_IND], OF_X[OF_ANGLE_IND]); + + DEBUG_PRINT("Angles (deg): ahrs = %f, ekf = %f.\n", (180.0f / M_PI)*eulers->phi, (180.0f / M_PI)*OF_X[OF_ANGLE_IND]); + + DEBUG_PRINT("P before correction:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, P); + + // P_k1_k1 = (eye(Nx) - K_k1*Hx)*P_k1_k*(eye(Nx) - K_k1*Hx)' + K_k1*R*K_k1'; % Joseph form of the covariance update equation + float _KJac[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(KJac, _KJac, N_STATES_OF_KF); + float_mat_mul(KJac, K, Jac, N_STATES_OF_KF, N_MEAS_OF_KF, N_STATES_OF_KF); + + float _eye[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(eye, _eye, N_STATES_OF_KF); + float_mat_diagonal_scal(eye, 1.0, N_STATES_OF_KF); + DEBUG_PRINT("eye:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, eye); + + float _eKJac[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(eKJac, _eKJac, N_STATES_OF_KF); + float_mat_diff(eKJac, eye, KJac, N_STATES_OF_KF, N_STATES_OF_KF); + DEBUG_PRINT("eKJac:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, eKJac); + + float _eKJacT[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(eKJacT, _eKJacT, N_STATES_OF_KF); + float_mat_transpose(eKJacT, eKJac, N_STATES_OF_KF, N_STATES_OF_KF); + // (eye(Nx) - K_k1*Hx)*P_k1_k*(eye(Nx) - K_k1*Hx)' + float _P_pre[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(P_pre, _P_pre, N_STATES_OF_KF); + float_mat_mul(P_pre, P, eKJacT, N_STATES_OF_KF, N_STATES_OF_KF, N_STATES_OF_KF); + float_mat_mul(P, eKJac, P_pre, N_STATES_OF_KF, N_STATES_OF_KF, N_STATES_OF_KF); + DEBUG_PRINT("eKJac * P *eKJacT:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, P); + + // K_k1*R*K_k1' + // TODO: check all MAKE_MATRIX that they mention the number of ROWS! + float _KT[N_MEAS_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(KT, _KT, N_MEAS_OF_KF); + float_mat_transpose(KT, K, N_STATES_OF_KF, N_MEAS_OF_KF); + float _RKT[N_MEAS_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(RKT, _RKT, N_MEAS_OF_KF); + float_mat_mul(RKT, R, KT, N_MEAS_OF_KF, N_MEAS_OF_KF, N_STATES_OF_KF); + float _KRKT[N_STATES_OF_KF][N_STATES_OF_KF]; + MAKE_MATRIX_PTR(KRKT, _KRKT, N_STATES_OF_KF); + float_mat_mul(KRKT, K, RKT, N_STATES_OF_KF, N_MEAS_OF_KF, N_STATES_OF_KF); + DEBUG_PRINT("KRKT:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, KRKT); + + // summing the two parts: + float_mat_sum(P, P, KRKT, N_STATES_OF_KF, N_STATES_OF_KF); + + DEBUG_PRINT("P corrected:\n"); + DEBUG_MAT_PRINT(N_STATES_OF_KF, N_STATES_OF_KF, P); + float trace_P = 0.0f; + for (int i = 0; i < N_STATES_OF_KF; i++) { + trace_P += P[i][i]; + } + DEBUG_PRINT("trace P = %f\n", trace_P); + + // indicate that the measurement has been used: + ins_flow.new_flow_measurement = false; + } + + // update the time: + of_prev_time = of_time; + +} + +static void gyro_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t stamp, struct Int32Rates *gyro) +{ + ahrs_icq_last_stamp = stamp; +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for AHRS_ICQ propagation.") + /* timestamp in usec when last callback was received */ + static uint32_t last_stamp = 0; + + if (last_stamp > 0 && ahrs_icq.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ahrs_icq_propagate(gyro, dt); + set_body_state_from_quat(); + + // TODO: filter all gyro values + // For now only filter the roll gyro: + float current_rate = ((float)gyro->p); // TODO: is this correct? / INT32_RATE_FRAC; + ins_flow.lp_gyro_roll = lp_factor * ins_flow.lp_gyro_roll + (1 - lp_factor) * current_rate; + current_rate = ((float)gyro->q); + ins_flow.lp_gyro_pitch = lp_factor * ins_flow.lp_gyro_pitch + (1 - lp_factor) * current_rate; + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_ICQ propagation.") + PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + if (ahrs_icq.status == AHRS_ICQ_RUNNING) { + const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); + ahrs_icq_propagate(gyro, dt); + set_body_state_from_quat(); + } +#endif +} + +static void accel_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t __attribute__((unused)) stamp, + struct Int32Vect3 *accel) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.") + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_icq.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ahrs_icq_update_accel(accel, dt); + set_body_state_from_quat(); + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.") + PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + if (ahrs_icq.is_aligned) { + const float dt = 1. / (AHRS_CORRECT_FREQUENCY); + ahrs_icq_update_accel(accel, dt); + set_body_state_from_quat(); + } +#endif +} + +/* +static void mag_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t __attribute__((unused)) stamp, + struct Int32Vect3 *mag) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat mag update.") + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_icq.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + //ahrs_icq_update_mag(mag, dt); + //set_body_state_from_quat(); + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS int_cmpl_quat mag update.") + PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + if (ahrs_icq.is_aligned) { + const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); + ahrs_icq_update_mag(mag, dt); + set_body_state_from_quat(); + } +#endif +} + +static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h) +{ + //VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(h->x), MAG_BFP_OF_REAL(h->y), + // MAG_BFP_OF_REAL(h->z)); +} +*/ + +/** Rotate angles and rates from imu to body frame and set state */ +static void set_body_state_from_quat(void) +{ + /* Compute LTP to BODY quaternion */ + struct Int32Quat ltp_to_body_quat = ahrs_icq.ltp_to_body_quat; + //struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu); + //int32_quat_comp_inv(<p_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat); + + if (use_filter < USE_ANGLE) { + // Use the orientation as is: + stateSetNedToBodyQuat_i(<p_to_body_quat); + } else { + + // get Euler angles: + struct OrientationReps orient; + orient.status = 1 << ORREP_QUAT_I; + orient.quat_i = ltp_to_body_quat; + struct FloatEulers *eulers = orientationGetEulers_f(&orient); + + // set roll angle with value from the filter: + GT_phi = eulers->phi; + eulers->phi = OF_X[OF_ANGLE_IND]; + if (OF_TWO_DIM) { + GT_theta = eulers->theta; + //printf("Real theta = %f, setting theta to %f.\n", eulers->theta, OF_X[OF_THETA_IND]); + eulers->theta = OF_X[OF_THETA_IND]; + } + + // Transform the Euler representation to Int32Quat and set the state: + struct OrientationReps orient_euler; + orient_euler.status = 1 << ORREP_EULER_F; + orient_euler.eulers_f = (*eulers); + + struct Int32Quat *quat_i_adapted = orientationGetQuat_i(&orient_euler); + stateSetNedToBodyQuat_i(quat_i_adapted); + } + + /* compute body rates */ + struct Int32Rates body_rate = ahrs_icq.body_rate; + // struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); + // int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate); + /* Set state */ + stateSetBodyRates_i(&body_rate); + +} + +static void ins_rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act) +{ + ins_flow.RPM_num_act = num_act; + for (int i = 0; i < num_act; i++) { + ins_flow.RPM[i] = rpm[i]; + } +} + + +/* Update INS based on GPS information */ +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp, + struct GpsState *gps_s) +{ + + if (gps_s->fix < GPS_FIX_3D) { + return; + } + if (!ins_flow.ltp_initialized) { + ins_reset_local_origin(); + } + + ahrs_icq_update_gps(gps_s); + + /* simply scale and copy pos/speed from gps */ + struct NedCoor_i gps_pos_cm_ned; + ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_flow.ltp_def, &gps_s->ecef_pos); + INT32_VECT3_SCALE_2(ins_flow.ltp_pos, gps_pos_cm_ned, + INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + + if (use_filter >= USE_HEIGHT) { + //struct NedCoor_f* position = stateGetPositionNed_f(); + int32_t z_Ned_i_filter = - (int32_t)((OF_X[OF_Z_IND] * INT32_POS_OF_CM_NUM * 100) / INT32_POS_OF_CM_DEN); + //printf("Z true: %f / %d, Z filter: %f / %d. (float / int32_t)\n", position->z, ins_flow.ltp_pos.z, OF_X[OF_Z_IND], z_Ned_i); + ins_flow.ltp_pos.z = z_Ned_i_filter; + } + + stateSetPositionNed_i(&ins_flow.ltp_pos); + + + + struct NedCoor_i gps_speed_cm_s_ned; + ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_flow.ltp_def, &gps_s->ecef_vel); + INT32_VECT3_SCALE_2(ins_flow.ltp_speed, gps_speed_cm_s_ned, + INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); + + if (use_filter >= USE_VELOCITY) { + // get NED to body rotation matrix: + struct FloatRMat *NTB = stateGetNedToBodyRMat_f(); + // get transpose (inverse): + struct FloatRMat BTN; + float_rmat_inv(&BTN, NTB); + + // the velocities from the filter are rotated from the body to the inertial frame: + struct FloatVect3 NED_velocities, body_velocities; + body_velocities.x = 0.0f; // filter does not determine this yet + body_velocities.y = OF_X[OF_V_IND]; + body_velocities.z = 0.0f; + /* + if(CONSTANT_ALT_FILTER) { + body_velocities.z = 0.0f; + } + else { + body_velocities.z = OF_X[OF_Z_DOT_IND]; + }*/ + float_rmat_vmult(&NED_velocities, &BTN, &body_velocities); + // TODO: also estimate vx, so that we can just use the rotated vector: + // For now, we need to keep the x, and y body axes aligned with the global ones. + // printf("Original speed y = %d, ", ins_flow.ltp_speed.y); + ins_flow.ltp_speed.y = (int32_t)((NED_velocities.y * INT32_SPEED_OF_CM_S_NUM * 100) / INT32_SPEED_OF_CM_S_DEN); + if (!CONSTANT_ALT_FILTER) { ins_flow.ltp_speed.z = -(int32_t)((NED_velocities.z * INT32_SPEED_OF_CM_S_NUM * 100) / INT32_SPEED_OF_CM_S_DEN); } + // printf("Changed speed y = %d (%f in float)\n", ins_flow.ltp_speed.y, NED_velocities.y); + } + + stateSetSpeedNed_i(&ins_flow.ltp_speed); + + /* + bool vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT); + uint8_t nsats = gps_s->num_sv; + */ +} + +static void aligner_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t stamp __attribute__((unused)), + struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag) +{ + if (!ahrs_icq.is_aligned) { + if (ahrs_icq_align(lp_gyro, lp_accel, lp_mag)) { + set_body_state_from_quat(); + } + } +} + + +/* Save the Body to IMU information */ +//// TODO: Is this code still necessary? +//static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), +// struct FloatQuat *q_b2i_f) +//{ +// ahrs_icq_set_body_to_imu_quat(q_b2i_f); +//} diff --git a/sw/airborne/modules/ins/ins_flow.h b/sw/airborne/modules/ins/ins_flow.h new file mode 100644 index 0000000000..9c72ac4fa0 --- /dev/null +++ b/sw/airborne/modules/ins/ins_flow.h @@ -0,0 +1,169 @@ +/* + * Copyright (C) 2021 Guido de Croon + * + * 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. + */ + +/** + * @file subsystems/ins/ins_flow.h + * + * "Inertial" navigation system. + */ + +#ifndef INS_FLOW_H +#define INS_FLOW_H + +#ifdef __cplusplus +extern "C" { +#endif + +#define CONSTANT_ALT_FILTER 1 +#define OF_DRAG 1 +// Only for constant alt for now! +#define OF_TWO_DIM 1 +// Only for changing alt +#define OF_THRUST_BIAS 0 +// Whether to use gyros: +#define OF_USE_GYROS 1 +// Predicting gyros with roll command and optic flow (normally 0): +#define PREDICT_GYROS 0 + +#if CONSTANT_ALT_FILTER == 1 + +#if OF_TWO_DIM == 0 + + +#define OF_V_IND 0 +#define OF_ANGLE_IND 1 +#define OF_Z_IND 2 +#define OF_ANGLE_DOT_IND 3 +#if OF_USE_GYROS == 1 +#define N_MEAS_OF_KF 3 +#else +// gyros not used at all +#define N_MEAS_OF_KF 2 +#endif + +#define OF_THETA_IND -1 +#define OF_VX_IND -1 + +#if OF_THRUST_BIAS == 0 +#define N_STATES_OF_KF 4 +#define OF_THRUST_BIAS_IND -1 +#else +// does this work with thrust bias? +#define N_STATES_OF_KF 5 +#define OF_THRUST_BIAS_IND 4 +#endif + +#else +#define N_STATES_OF_KF 6 +#define OF_V_IND 0 +#define OF_ANGLE_IND 1 +#define OF_Z_IND 2 +#define OF_THETA_IND 3 +#define OF_VX_IND 4 +#define OF_ANGLE_DOT_IND 5 +// TODO: also a theta dot ind? + + +#define OF_THRUST_BIAS_IND -1 + +// the third measurement here is the other lateral flow: +#if OF_USE_GYROS == 1 +// gyros used in the prediction and measurement +#define N_MEAS_OF_KF 4 +#else +// gyros not used at all +#define N_MEAS_OF_KF 3 +#endif +#endif + +#define OF_Z_DOT_IND -1 +#else +#if OF_THRUST_BIAS == 0 +#define N_STATES_OF_KF 5 +#define OF_THRUST_BIAS_IND -1 +#else +#define N_STATES_OF_KF 6 +#define OF_THRUST_BIAS_IND 5 +#endif + +#define OF_V_IND 0 +#define OF_ANGLE_IND 1 +#define OF_ANGLE_DOT_IND 2 +#define OF_Z_IND 3 +#define OF_Z_DOT_IND 4 + +#define OF_THETA_IND -1 +#define OF_VX_IND -1 + + +#if OF_USE_GYROS == 1 +// gyros used in the prediction and measurement +#define N_MEAS_OF_KF 3 +#else +// gyros not used at all +#define N_MEAS_OF_KF 2 +#endif + +#endif + +// TODO: make these parameters in the estimation scheme: +#define OF_TB_Q 0.02 +#define OF_TB_P 0.5 + + +#define OF_LAT_FLOW_IND 0 +#define OF_DIV_FLOW_IND 1 +#define OF_RATE_IND 2 +#if OF_USE_GYROS == 1 +#define OF_LAT_FLOW_X_IND 3 +#else +#define OF_LAT_FLOW_X_IND 2 +#endif + + +// use filter to different extents: +#define USE_ANGLE 1 +#define USE_VELOCITY 2 +#define USE_HEIGHT 3 + + +#include "modules/ahrs/ahrs.h" +#include "modules/ahrs/ahrs_int_cmpl_quat.h" +#include "modules/ahrs/ahrs_aligner.h" +#include "modules/ins/ins.h" + +extern void ins_flow_init(void); +extern void ins_flow_update(void); + +extern float OF_X[N_STATES_OF_KF]; +extern bool reset_filter; +extern bool run_filter; +extern int use_filter; +extern float thrust_factor; +extern float GT_phi; +extern float GT_theta; + + +#ifdef __cplusplus +} +#endif + +#endif /* INS_FLOW_H */