Merge pull request #1221 from ggregory8/vision_estimate

Separate position correction for vision estimate out from GPS logic. Add weight parameters. Fixes valid position indication.

3 files changed, 98 insertions, 18 deletions

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 71abcc170..06a81e3fe 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -235,6 +235,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	float eph_flow = 1.0f;
 
+	float eph_vision = 0.5f;
+	float epv_vision = 0.5f;
+
 	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
 	memset(x_est_prev, 0, sizeof(x_est_prev));
 	memset(y_est_prev, 0, sizeof(y_est_prev));
@@ -281,6 +284,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	};
 	float w_gps_xy = 1.0f;
 	float w_gps_z = 1.0f;
+	
+	float corr_vision[3][2] = {
+		{ 0.0f, 0.0f },		// N (pos, vel)
+		{ 0.0f, 0.0f },		// E (pos, vel)
+		{ 0.0f, 0.0f },		// D (pos, vel)
+	};
+	
 	float corr_sonar = 0.0f;
 	float corr_sonar_filtered = 0.0f;
 
@@ -637,27 +647,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						x_est[1] = vision.vx;
 						y_est[0] = vision.y;
 						y_est[1] = vision.vy;
-						z_est[0] = vision.z;
-						z_est[1] = vision.vz;
-
+						/* only reset the z estimate if the z weight parameter is not zero */ 
+						if (params.w_z_vision_p > MIN_VALID_W)
+						{
+							z_est[0] = vision.z;
+							z_est[1] = vision.vz;
+						}
+						
 						vision_valid = true;
+						warnx("VISION estimate valid");
+						mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
 					}
 
 					/* calculate correction for position */
-					corr_gps[0][0] = vision.x - x_est[0];
-					corr_gps[1][0] = vision.y - y_est[0];
-					corr_gps[2][0] = vision.z - z_est[0];
+					corr_vision[0][0] = vision.x - x_est[0];
+					corr_vision[1][0] = vision.y - y_est[0];
+					corr_vision[2][0] = vision.z - z_est[0];
 
 					/* calculate correction for velocity */
-					corr_gps[0][1] = vision.vx - x_est[1];
-					corr_gps[1][1] = vision.vy - y_est[1];
-					corr_gps[2][1] = vision.vz - z_est[1];
-
-					eph = 0.05f;
-					epv = 0.05f;
-
-					w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph);
-					w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv);
+					corr_vision[0][1] = vision.vx - x_est[1];
+					corr_vision[1][1] = vision.vy - y_est[1];
+					corr_vision[2][1] = vision.vz - z_est[1];
 
 				}
 			}
@@ -810,12 +820,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		/* use GPS if it's valid and reference position initialized */
-		bool use_gps_xy = (ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W) || vision_valid;
-		bool use_gps_z = (ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W) || vision_valid;
+		bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
+		bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
+		/* use VISION if it's valid and has a valid weight parameter */
+		bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
+		bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
 		/* use flow if it's valid and (accurate or no GPS available) */
 		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
 
-		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || vision_valid;
+		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy;
 
 		bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
 
@@ -834,6 +847,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
 		float w_z_gps_p = params.w_z_gps_p * w_gps_z;
 
+		float w_xy_vision_p = params.w_xy_vision_p;
+		float w_xy_vision_v = params.w_xy_vision_v;
+		float w_z_vision_p = params.w_z_vision_p;
+
 		/* reduce GPS weight if optical flow is good */
 		if (use_flow && flow_accurate) {
 			w_xy_gps_p *= params.w_gps_flow;
@@ -874,6 +891,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* accelerometer bias correction for VISION (use buffered rotation matrix) */
+		accel_bias_corr[0] = 0.0f;
+		accel_bias_corr[1] = 0.0f;
+		accel_bias_corr[2] = 0.0f;
+
+		if (use_vision_xy) {
+			accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;
+			accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;
+			accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;
+			accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;
+		}
+
+		if (use_vision_z) {
+			accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
+		}
+
+		/* transform error vector from NED frame to body frame */
+		for (int i = 0; i < 3; i++) {
+			float c = 0.0f;
+
+			for (int j = 0; j < 3; j++) {
+				c += att.R[j][i] * accel_bias_corr[j];
+			}
+
+			if (isfinite(c)) {
+				acc_bias[i] += c * params.w_acc_bias * dt;
+			}
+		}
+
 		/* accelerometer bias correction for flow and baro (assume that there is no delay) */
 		accel_bias_corr[0] = 0.0f;
 		accel_bias_corr[1] = 0.0f;
@@ -916,10 +962,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
 		}
 
+		if (use_vision_z) {
+			epv = fminf(epv, epv_vision);
+			inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
+		}
+
 		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
 			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 			memcpy(z_est, z_est_prev, sizeof(z_est));
 			memset(corr_gps, 0, sizeof(corr_gps));
+			memset(corr_vision, 0, sizeof(corr_vision));
 			corr_baro = 0;
 
 		} else {
@@ -957,11 +1009,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 			}
 
+			if (use_vision_xy) {
+				eph = fminf(eph, eph_vision);
+
+				inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p);
+				inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p);
+
+				if (w_xy_vision_v > MIN_VALID_W) {
+					inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v);
+					inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v);
+				}
+			}
+
 			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
 				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 				memcpy(x_est, x_est_prev, sizeof(x_est));
 				memcpy(y_est, y_est_prev, sizeof(y_est));
 				memset(corr_gps, 0, sizeof(corr_gps));
+				memset(corr_vision, 0, sizeof(corr_vision));
 				memset(corr_flow, 0, sizeof(corr_flow));
 
 			} else {
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index 0d9fbef27..90d8d2b50 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -43,9 +43,12 @@
 
 PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
@@ -64,9 +67,12 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 {
 	h->w_z_baro = param_find("INAV_W_Z_BARO");
 	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
+	h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
 	h->w_z_sonar = param_find("INAV_W_Z_SONAR");
 	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
 	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
+	h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
+	h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");
 	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
 	h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
 	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
@@ -88,9 +94,12 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 {
 	param_get(h->w_z_baro, &(p->w_z_baro));
 	param_get(h->w_z_gps_p, &(p->w_z_gps_p));
+	param_get(h->w_z_vision_p, &(p->w_z_vision_p));
 	param_get(h->w_z_sonar, &(p->w_z_sonar));
 	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
 	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
+	param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
+	param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));
 	param_get(h->w_xy_flow, &(p->w_xy_flow));
 	param_get(h->w_xy_res_v, &(p->w_xy_res_v));
 	param_get(h->w_gps_flow, &(p->w_gps_flow));
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index 08098d6c3..d0a65e42e 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -44,9 +44,12 @@
 struct position_estimator_inav_params {
 	float w_z_baro;
 	float w_z_gps_p;
+	float w_z_vision_p;
 	float w_z_sonar;
 	float w_xy_gps_p;
 	float w_xy_gps_v;
+	float w_xy_vision_p;
+	float w_xy_vision_v;
 	float w_xy_flow;
 	float w_xy_res_v;
 	float w_gps_flow;
@@ -65,9 +68,12 @@ struct position_estimator_inav_params {
 struct position_estimator_inav_param_handles {
 	param_t w_z_baro;
 	param_t w_z_gps_p;
+	param_t w_z_vision_p;
 	param_t w_z_sonar;
 	param_t w_xy_gps_p;
 	param_t w_xy_gps_v;
+	param_t w_xy_vision_p;
+	param_t w_xy_vision_v;
 	param_t w_xy_flow;
 	param_t w_xy_res_v;
 	param_t w_gps_flow;