zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

position_estimator_inav: remove acceleration from state and INAV_W_XXX_ACC parameters, more NaN checks

sbg
Anton Babushkin 11 years ago
parent
36495cdb62
commit
9a49636f6a
5 changed files with 38 additions and 57 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 15
      src/modules/position_estimator_inav/inertial_filter.c
  2. 2
      src/modules/position_estimator_inav/inertial_filter.h
  3. 68
      src/modules/position_estimator_inav/position_estimator_inav_main.c
  4. 6
      src/modules/position_estimator_inav/position_estimator_inav_params.c
  5. 4
      src/modules/position_estimator_inav/position_estimator_inav_params.h

15
src/modules/position_estimator_inav/inertial_filter.c
Unescape View File

@ -9,15 +9,18 @@
#include "inertial_filter.h" #include "inertial_filter.h"
void inertial_filter_predict(float dt, float x[3]) void inertial_filter_predict(float dt, float x[2], float acc)
{ {
if (isfinite(dt)) { if (isfinite(dt)) {
x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; if (!isfinite(acc)) {
x[1] += x[2] * dt; acc = 0.0f;
}
x[0] += x[1] * dt + acc * dt * dt / 2.0f;
x[1] += acc * dt;
} }
} }
void inertial_filter_correct(float e, float dt, float x[3], int i, float w) void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
{ {
if (isfinite(e) && isfinite(w) && isfinite(dt)) { if (isfinite(e) && isfinite(w) && isfinite(dt)) {
float ewdt = e * w * dt; float ewdt = e * w * dt;
@ -25,10 +28,6 @@ void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
if (i == 0) { if (i == 0) {
x[1] += w * ewdt; x[1] += w * ewdt;
x[2] += w * w * ewdt / 3.0;
} else if (i == 1) {
x[2] += w * ewdt;
} }
} }
} }

2
src/modules/position_estimator_inav/inertial_filter.h
Unescape View File

@ -8,6 +8,6 @@
#include <stdbool.h> #include <stdbool.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
void inertial_filter_predict(float dt, float x[3]); void inertial_filter_predict(float dt, float x[3], float acc);
void inertial_filter_correct(float e, float dt, float x[3], int i, float w); void inertial_filter_correct(float e, float dt, float x[3], int i, float w);

68
src/modules/position_estimator_inav/position_estimator_inav_main.c
Unescape View File

@ -168,15 +168,15 @@ int position_estimator_inav_main(int argc, char *argv[])
exit(1); exit(1);
} }
void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
{ {
FILE *f = fopen("/fs/microsd/inav.log", "a"); FILE *f = fopen("/fs/microsd/inav.log", "a");
if (f) { if (f) {
char *s = malloc(256); char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]); unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]);
fwrite(s, 1, n, f); fwrite(s, 1, n, f);
n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
fwrite(s, 1, n, f); fwrite(s, 1, n, f);
free(s); free(s);
} }
@ -195,14 +195,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[inav] started"); mavlink_log_info(mavlink_fd, "[inav] started");
float x_est[3] = { 0.0f, 0.0f, 0.0f }; float x_est[2] = { 0.0f, 0.0f }; // pos, vel
float y_est[3] = { 0.0f, 0.0f, 0.0f }; float y_est[2] = { 0.0f, 0.0f }; // pos, vel
float z_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[2] = { 0.0f, 0.0f }; // pos, vel
float eph = 1.0; float eph = 1.0;
float epv = 1.0; float epv = 1.0;
float x_est_prev[3], y_est_prev[3], z_est_prev[3]; float x_est_prev[2], y_est_prev[2], z_est_prev[2];
memset(x_est_prev, 0, sizeof(x_est_prev)); memset(x_est_prev, 0, sizeof(x_est_prev));
memset(y_est_prev, 0, sizeof(y_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev));
memset(z_est_prev, 0, sizeof(z_est_prev)); memset(z_est_prev, 0, sizeof(z_est_prev));
@ -241,7 +241,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float corr_baro = 0.0f; // D float corr_baro = 0.0f; // D
float corr_gps[3][2] = { float corr_gps[3][2] = {
@ -341,8 +341,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* mean calculation over several measurements */ /* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) { if (baro_init_cnt < baro_init_num) {
baro_offset += sensor.baro_alt_meter; if (isfinite(sensor.baro_alt_meter)) {
baro_init_cnt++; baro_offset += sensor.baro_alt_meter;
baro_init_cnt++;
}
} else { } else {
wait_baro = false; wait_baro = false;
@ -418,19 +420,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* transform acceleration vector from body frame to NED frame */ /* transform acceleration vector from body frame to NED frame */
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
accel_NED[i] = 0.0f; acc[i] = 0.0f;
for (int j = 0; j < 3; j++) { for (int j = 0; j < 3; j++) {
accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
} }
} }
corr_acc[0] = accel_NED[0] - x_est[2]; acc[2] += CONSTANTS_ONE_G;
corr_acc[1] = accel_NED[1] - y_est[2];
corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
} else { } else {
memset(corr_acc, 0, sizeof(corr_acc)); memset(acc, 0, sizeof(acc));
} }
accel_timestamp = sensor.accelerometer_timestamp; accel_timestamp = sensor.accelerometer_timestamp;
@ -628,11 +628,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* set position estimate to (0, 0, 0), use GPS velocity for XY */ /* set position estimate to (0, 0, 0), use GPS velocity for XY */
x_est[0] = 0.0f; x_est[0] = 0.0f;
x_est[1] = gps.vel_n_m_s; x_est[1] = gps.vel_n_m_s;
x_est[2] = accel_NED[0];
y_est[0] = 0.0f; y_est[0] = 0.0f;
y_est[1] = gps.vel_e_m_s; y_est[1] = gps.vel_e_m_s;
z_est[0] = 0.0f; z_est[0] = 0.0f;
y_est[2] = accel_NED[1];
local_pos.ref_lat = lat; local_pos.ref_lat = lat;
local_pos.ref_lon = lon; local_pos.ref_lon = lon;
@ -655,10 +653,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (reset_est) { if (reset_est) {
x_est[0] = gps_proj[0]; x_est[0] = gps_proj[0];
x_est[1] = gps.vel_n_m_s; x_est[1] = gps.vel_n_m_s;
x_est[2] = accel_NED[0];
y_est[0] = gps_proj[1]; y_est[0] = gps_proj[1];
y_est[1] = gps.vel_e_m_s; y_est[1] = gps.vel_e_m_s;
y_est[2] = accel_NED[1];
} }
/* calculate correction for position */ /* calculate correction for position */
@ -796,26 +792,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
c += att.R[j][i] * accel_bias_corr[j]; c += att.R[j][i] * accel_bias_corr[j];
} }
acc_bias[i] += c * params.w_acc_bias * dt; if (isfinite(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
} }
/* inertial filter prediction for altitude */ /* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est); inertial_filter_predict(dt, z_est, acc[2]);
if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", 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)); memcpy(z_est, z_est_prev, sizeof(z_est));
} }
/* inertial filter correction for altitude */ /* inertial filter correction for altitude */
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { 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, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); 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)); memcpy(z_est, z_est_prev, sizeof(z_est));
memset(corr_acc, 0, sizeof(corr_acc));
memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_gps, 0, sizeof(corr_gps));
corr_baro = 0; corr_baro = 0;
@ -825,19 +821,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (can_estimate_xy) { if (can_estimate_xy) {
/* inertial filter prediction for position */ /* inertial filter prediction for position */
inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, x_est, acc[0]);
inertial_filter_predict(dt, y_est); inertial_filter_predict(dt, y_est, acc[1]);
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); write_debug_log("BAD ESTIMATE AFTER PREDICTION", 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(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est)); memcpy(y_est, y_est_prev, sizeof(y_est));
} }
/* inertial filter correction for position */ /* inertial filter correction for position */
inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc);
inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc);
if (use_flow) { if (use_flow) {
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
@ -853,11 +846,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
} }
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { 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, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); 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(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est)); memcpy(y_est, y_est_prev, sizeof(y_est));
memset(corr_acc, 0, sizeof(corr_acc));
memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_flow, 0, sizeof(corr_flow)); memset(corr_flow, 0, sizeof(corr_flow));

6
src/modules/position_estimator_inav/position_estimator_inav_params.c
Unescape View File

@ -42,11 +42,9 @@
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); 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_GPS_P, 0.005f);
PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); 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_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
@ -62,11 +60,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
{ {
h->w_z_baro = param_find("INAV_W_Z_BARO"); 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_gps_p = param_find("INAV_W_Z_GPS_P");
h->w_z_acc = param_find("INAV_W_Z_ACC");
h->w_z_sonar = param_find("INAV_W_Z_SONAR"); 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_p = param_find("INAV_W_XY_GPS_P");
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
h->w_xy_acc = param_find("INAV_W_XY_ACC");
h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_flow = param_find("INAV_W_XY_FLOW");
h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
@ -85,11 +81,9 @@ 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_baro, &(p->w_z_baro));
param_get(h->w_z_gps_p, &(p->w_z_gps_p)); param_get(h->w_z_gps_p, &(p->w_z_gps_p));
param_get(h->w_z_acc, &(p->w_z_acc));
param_get(h->w_z_sonar, &(p->w_z_sonar)); 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_p, &(p->w_xy_gps_p));
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
param_get(h->w_xy_acc, &(p->w_xy_acc));
param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_xy_flow, &(p->w_xy_flow));
param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_gps_flow, &(p->w_gps_flow));
param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->w_acc_bias, &(p->w_acc_bias));

4
src/modules/position_estimator_inav/position_estimator_inav_params.h
Unescape View File

@ -43,11 +43,9 @@
struct position_estimator_inav_params { struct position_estimator_inav_params {
float w_z_baro; float w_z_baro;
float w_z_gps_p; float w_z_gps_p;
float w_z_acc;
float w_z_sonar; float w_z_sonar;
float w_xy_gps_p; float w_xy_gps_p;
float w_xy_gps_v; float w_xy_gps_v;
float w_xy_acc;
float w_xy_flow; float w_xy_flow;
float w_gps_flow; float w_gps_flow;
float w_acc_bias; float w_acc_bias;
@ -63,11 +61,9 @@ struct position_estimator_inav_params {
struct position_estimator_inav_param_handles { struct position_estimator_inav_param_handles {
param_t w_z_baro; param_t w_z_baro;
param_t w_z_gps_p; param_t w_z_gps_p;
param_t w_z_acc;
param_t w_z_sonar; param_t w_z_sonar;
param_t w_xy_gps_p; param_t w_xy_gps_p;
param_t w_xy_gps_v; param_t w_xy_gps_v;
param_t w_xy_acc;
param_t w_xy_flow; param_t w_xy_flow;
param_t w_gps_flow; param_t w_gps_flow;
param_t w_acc_bias; param_t w_acc_bias;

Write Preview
Loading…
Cancel
Save