Use GPS velocity in position estimator

5 files changed, 45 insertions, 13 deletions

diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c
index 64031ee7b..390e1b720 100644
--- a/src/modules/position_estimator_inav/kalman_filter_inertial.c
+++ b/src/modules/position_estimator_inav/kalman_filter_inertial.c
@@ -12,7 +12,7 @@ void kalman_filter_inertial_predict(float dt, float x[3]) {
 	x[1] += x[2] * dt;
 }
 
-void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) {
+void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]) {
 	float y[2];
 	// y = z - H x
 	y[0] = z[0] - x[0];
@@ -25,3 +25,18 @@ void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool u
 		}
 	}
 }
+
+void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) {
+	float y[2];
+	// y = z - H x
+	y[0] = z[0] - x[0];
+	y[1] = z[1] - x[1];
+	y[2] = z[2] - x[2];
+	// x = x + K * y
+	for (int i = 0; i < 3; i++) {		// Row
+		for (int j = 0; j < 3; j++) {	// Column
+			if (use[j])
+				x[i] += k[i][j] * y[j];
+		}
+	}
+}
diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h
index 3e5134c33..d34f58298 100644
--- a/src/modules/position_estimator_inav/kalman_filter_inertial.h
+++ b/src/modules/position_estimator_inav/kalman_filter_inertial.h
@@ -9,4 +9,6 @@
 
 void kalman_filter_inertial_predict(float dt, float x[3]);
 
-void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]);
+void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]);
+
+void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]);
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 2b485f895..6ecdfe01d 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -370,7 +370,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 			}
 			if (use_z[0] || use_z[1]) {
 				/* correction */
-				kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z);
+				kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z);
 			}
 
 			if (params.use_gps) {
@@ -378,23 +378,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 				kalman_filter_inertial_predict(dt, x_est);
 				kalman_filter_inertial_predict(dt, y_est);
 				/* prepare vectors for kalman filter correction */
-				float x_meas[2];	// position, acceleration
-				float y_meas[2];	// position, acceleration
-				bool use_xy[2] = { false, false };
+				float x_meas[3];	// position, velocity, acceleration
+				float y_meas[3];	// position, velocity, acceleration
+				bool use_xy[3] = { false, false, false };
 				if (gps_updated) {
 					x_meas[0] = local_pos_gps[0];
 					y_meas[0] = local_pos_gps[1];
+					x_meas[1] = gps.vel_n_m_s;
+					y_meas[1] = gps.vel_e_m_s;
 					use_xy[0] = true;
+					use_xy[1] = true;
 				}
 				if (accelerometer_updated) {
-					x_meas[1] = accel_NED[0];
-					y_meas[1] = accel_NED[1];
-					use_xy[1] = true;
+					x_meas[2] = accel_NED[0];
+					y_meas[2] = accel_NED[1];
+					use_xy[2] = true;
 				}
-				if (use_xy[0] || use_xy[1]) {
+				if (use_xy[0] || use_xy[2]) {
 					/* correction */
-					kalman_filter_inertial_update(x_est, x_meas, params.k_pos, use_xy);
-					kalman_filter_inertial_update(y_est, y_meas, params.k_pos, use_xy);
+					kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy);
+					kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy);
 				}
 			}
 		}
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 8466bcd0a..20142b70c 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -51,10 +51,13 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f);
 
 PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_K_POS_02, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_K_POS_12, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_K_POS_22, 0.0f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h) {
 	h->use_gps = param_find("INAV_USE_GPS");
@@ -68,10 +71,13 @@ int parameters_init(struct position_estimator_inav_param_handles *h) {
 
 	h->k_pos_00 = param_find("INAV_K_POS_00");
 	h->k_pos_01 = param_find("INAV_K_POS_01");
+	h->k_pos_02 = param_find("INAV_K_POS_02");
 	h->k_pos_10 = param_find("INAV_K_POS_10");
 	h->k_pos_11 = param_find("INAV_K_POS_11");
+	h->k_pos_12 = param_find("INAV_K_POS_12");
 	h->k_pos_20 = param_find("INAV_K_POS_20");
 	h->k_pos_21 = param_find("INAV_K_POS_21");
+	h->k_pos_22 = param_find("INAV_K_POS_22");
 
 	return OK;
 }
@@ -88,10 +94,13 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 
 	param_get(h->k_pos_00, &(p->k_pos[0][0]));
 	param_get(h->k_pos_01, &(p->k_pos[0][1]));
+	param_get(h->k_pos_02, &(p->k_pos[0][2]));
 	param_get(h->k_pos_10, &(p->k_pos[1][0]));
 	param_get(h->k_pos_11, &(p->k_pos[1][1]));
+	param_get(h->k_pos_12, &(p->k_pos[1][2]));
 	param_get(h->k_pos_20, &(p->k_pos[2][0]));
 	param_get(h->k_pos_21, &(p->k_pos[2][1]));
+	param_get(h->k_pos_22, &(p->k_pos[2][2]));
 
 	return OK;
 }
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 8cdc2e10f..c0e0042b6 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -43,7 +43,7 @@
 struct position_estimator_inav_params {
 	int use_gps;
 	float k_alt[3][2];
-	float k_pos[3][2];
+	float k_pos[3][3];
 };
 
 struct position_estimator_inav_param_handles {
@@ -58,10 +58,13 @@ struct position_estimator_inav_param_handles {
 
 	param_t k_pos_00;
 	param_t k_pos_01;
+	param_t k_pos_02;
 	param_t k_pos_10;
 	param_t k_pos_11;
+	param_t k_pos_12;
 	param_t k_pos_20;
 	param_t k_pos_21;
+	param_t k_pos_22;
 };
 
 /**