aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/position_estimator_inav/position_estimator_inav_main.c51
-rw-r--r--apps/position_estimator_inav/position_estimator_inav_params.c35
-rw-r--r--apps/position_estimator_inav/position_estimator_inav_params.h10
3 files changed, 74 insertions, 22 deletions
diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c
index 071ec6ad4..2b485f895 100644
--- a/apps/position_estimator_inav/position_estimator_inav_main.c
+++ b/apps/position_estimator_inav/position_estimator_inav_main.c
@@ -261,6 +261,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
while (!thread_should_exit) {
bool accelerometer_updated = false;
bool baro_updated = false;
+ bool gps_updated = false;
+ float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f };
+
int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate
if (ret < 0) {
/* poll error */
@@ -322,14 +325,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
/* vehicle GPS position */
if (fds[4].revents & POLLIN) {
/* new GPS value */
- orb_copy(ORB_ID(vehicle_gps_position),
- vehicle_gps_position_sub, &gps);
- static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */
+ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
/* Project gps lat lon (Geographic coordinate system) to plane */
map_projection_project(((double) (gps.lat)) * 1e-7,
((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]),
&(local_pos_gps[1]));
local_pos_gps[2] = (float) (gps.alt * 1e-3);
+ gps_updated = true;
pos.valid = gps.fix_type >= 3;
gps_updates++;
}
@@ -352,7 +354,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
}
}
accel_NED[2] += CONSTANTS_ONE_G;
- /* kalman filter prediction */
+
+ /* kalman filter for altitude */
kalman_filter_inertial_predict(dt, z_est);
/* prepare vectors for kalman filter correction */
float z_meas[2]; // position, acceleration
@@ -367,8 +370,32 @@ 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,
- use_z);
+ kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z);
+ }
+
+ if (params.use_gps) {
+ /* kalman filter for position */
+ 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 };
+ if (gps_updated) {
+ x_meas[0] = local_pos_gps[0];
+ y_meas[0] = local_pos_gps[1];
+ use_xy[0] = true;
+ }
+ if (accelerometer_updated) {
+ x_meas[1] = accel_NED[0];
+ y_meas[1] = accel_NED[1];
+ use_xy[1] = true;
+ }
+ if (use_xy[0] || use_xy[1]) {
+ /* 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);
+ }
}
}
if (verbose_mode) {
@@ -390,10 +417,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
}
if (t - pub_last > pub_interval) {
pub_last = t;
- pos.x = 0.0f;
- pos.vx = 0.0f;
- pos.y = 0.0f;
- pos.vy = 0.0f;
+ pos.x = x_est[0];
+ pos.vx = x_est[1];
+ pos.y = y_est[0];
+ pos.vy = y_est[1];
pos.z = z_est[0];
pos.vz = z_est[1];
pos.timestamp = hrt_absolute_time();
@@ -402,9 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
&& (isfinite(pos.vy))
&& (isfinite(pos.z))
&& (isfinite(pos.vz))) {
- orb_publish(ORB_ID(
- vehicle_local_position), vehicle_local_position_pub,
- &pos);
+ orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos);
}
}
}
diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c
index bb2b01481..8466bcd0a 100644
--- a/apps/position_estimator_inav/position_estimator_inav_params.c
+++ b/apps/position_estimator_inav/position_estimator_inav_params.c
@@ -40,8 +40,6 @@
#include "position_estimator_inav_params.h"
-/* Kalman Filter covariances */
-/* gps measurement noise standard deviation */
PARAM_DEFINE_INT32(INAV_USE_GPS, 0);
PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f);
@@ -51,6 +49,13 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f);
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_10, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f);
+
int parameters_init(struct position_estimator_inav_param_handles *h) {
h->use_gps = param_find("INAV_USE_GPS");
@@ -61,18 +66,32 @@ int parameters_init(struct position_estimator_inav_param_handles *h) {
h->k_alt_20 = param_find("INAV_K_ALT_20");
h->k_alt_21 = param_find("INAV_K_ALT_21");
+ h->k_pos_00 = param_find("INAV_K_POS_00");
+ h->k_pos_01 = param_find("INAV_K_POS_01");
+ h->k_pos_10 = param_find("INAV_K_POS_10");
+ h->k_pos_11 = param_find("INAV_K_POS_11");
+ h->k_pos_20 = param_find("INAV_K_POS_20");
+ h->k_pos_21 = param_find("INAV_K_POS_21");
+
return OK;
}
int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) {
param_get(h->use_gps, &(p->use_gps));
- param_get(h->k_alt_00, &(p->k[0][0]));
- param_get(h->k_alt_01, &(p->k[0][1]));
- param_get(h->k_alt_10, &(p->k[1][0]));
- param_get(h->k_alt_11, &(p->k[1][1]));
- param_get(h->k_alt_20, &(p->k[2][0]));
- param_get(h->k_alt_21, &(p->k[2][1]));
+ param_get(h->k_alt_00, &(p->k_alt[0][0]));
+ param_get(h->k_alt_01, &(p->k_alt[0][1]));
+ param_get(h->k_alt_10, &(p->k_alt[1][0]));
+ param_get(h->k_alt_11, &(p->k_alt[1][1]));
+ param_get(h->k_alt_20, &(p->k_alt[2][0]));
+ param_get(h->k_alt_21, &(p->k_alt[2][1]));
+
+ 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_10, &(p->k_pos[1][0]));
+ param_get(h->k_pos_11, &(p->k_pos[1][1]));
+ param_get(h->k_pos_20, &(p->k_pos[2][0]));
+ param_get(h->k_pos_21, &(p->k_pos[2][1]));
return OK;
}
diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h
index eaa1bbc95..8cdc2e10f 100644
--- a/apps/position_estimator_inav/position_estimator_inav_params.h
+++ b/apps/position_estimator_inav/position_estimator_inav_params.h
@@ -42,7 +42,8 @@
struct position_estimator_inav_params {
int use_gps;
- float k[3][2];
+ float k_alt[3][2];
+ float k_pos[3][2];
};
struct position_estimator_inav_param_handles {
@@ -54,6 +55,13 @@ struct position_estimator_inav_param_handles {
param_t k_alt_11;
param_t k_alt_20;
param_t k_alt_21;
+
+ param_t k_pos_00;
+ param_t k_pos_01;
+ param_t k_pos_10;
+ param_t k_pos_11;
+ param_t k_pos_20;
+ param_t k_pos_21;
};
/**
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-09 10:10:24 +0000