fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 00:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Estimate accelerometers offset in position_estimator_inav

Browse Source
This commit is contained in:
Anton Babushkin
2013-04-24 18:42:34 +04:00
parent 325a70e0e7
commit 0c6eaae863
3 changed files with 53 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/position_estimator_inav/position_estimator_inav_main.c
+42 -32
View File
@@ -280,6 +280,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
static float x_est[3] = { 0.0f, 0.0f, 0.0f };
static float y_est[3] = { 0.0f, 0.0f, 0.0f };
static float z_est[3] = { 0.0f, 0.0f, 0.0f };
float accel_offs_est[3] = { 0.0f, 0.0f, 0.0f };
int baro_loop_cnt = 0;
int baro_loop_end = 70; /* measurement for 1 second */
@@ -471,36 +472,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
hrt_abstime t = hrt_absolute_time();
float dt = (t - last_time) / 1000000.0;
last_time = t;
/* calculate corrected acceleration vector in UAV frame */
float accel_corr[3];
acceleration_correct(accel_corr, sensor.accelerometer_raw,
params.acc_T, params.acc_offs);
/* transform acceleration vector from UAV frame to NED frame */
float accel_NED[3];
for (int i = 0; i < 3; i++) {
accel_NED[i] = 0.0f;
for (int j = 0; j < 3; j++) {
accel_NED[i] += att.R[i][j] * accel_corr[j];
if (att.R_valid) {
/* calculate corrected acceleration vector in UAV frame */
float accel_corr[3];
acceleration_correct(accel_corr, sensor.accelerometer_raw,
params.acc_T, params.acc_offs);
/* transform acceleration vector from UAV frame to NED frame */
float accel_NED[3];
for (int i = 0; i < 3; i++) {
accel_NED[i] = 0.0f;
for (int j = 0; j < 3; j++) {
accel_NED[i] += att.R[i][j] * (accel_corr[j] - accel_offs_est[j]);
}
}
accel_NED[2] += const_earth_gravity;
/* accelerometers offset drift correction: rotate acceleration error back to UAV frame and integrate */
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
/* the inverse of a rotation matrix is its transpose, just swap i and j */
accel_offs_est[i] += att.R[j][i] * accel_NED[j] * params.acc_offs_w * dt;
}
}
/* kalman filter prediction */
kalman_filter_inertial_predict(dt, z_est);
/* prepare vectors for kalman filter correction */
float z_meas[2]; // position, acceleration
bool use_z[2] = { false, false };
if (local_flag_baroINITdone && baro_updated) {
z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt
use_z[0] = true;
}
if (accelerometer_updated) {
z_meas[1] = accel_NED[2];
use_z[1] = true;
}
if (use_z[0] || use_z[1]) {
/* correction */
kalman_filter_inertial_update(z_est, z_meas, params.k,
use_z);
}
}
accel_NED[2] += const_earth_gravity;
/* kalman filter prediction */
kalman_filter_inertial_predict(dt, z_est);
/* prepare vectors for kalman filter correction */
float z_meas[2]; // position, acceleration
bool use_z[2] = { false, false };
if (local_flag_baroINITdone && baro_updated) {
z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt
use_z[0] = true;
}
if (accelerometer_updated) {
z_meas[1] = accel_NED[2];
use_z[1] = true;
}
if (use_z[0] || use_z[1]) {
/* correction */
kalman_filter_inertial_update(z_est, z_meas, params.k,
use_z);
}
if (verbose_mode) {
/* print updates rate */
@@ -521,9 +531,9 @@ 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.x = accel_offs_est[0];
pos.vx = accel_offs_est[1];
pos.y = accel_offs_est[2];
pos.vy = 0.0f;
pos.z = z_est[0];
pos.vz = z_est[1];
apps/position_estimator_inav/position_estimator_inav_params.c
+8
View File
@@ -39,6 +39,7 @@
*/
#include "position_estimator_inav_params.h"
#include "acceleration_transform.h"
/* Kalman Filter covariances */
/* gps measurement noise standard deviation */
@@ -51,6 +52,8 @@ 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_ACC_OFFS_W, 0.0f);
PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0);
PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0);
PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0);
@@ -75,6 +78,8 @@ 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->acc_offs_w = param_find("INAV_ACC_OFFS_W");
h->acc_offs_0 = param_find("INAV_ACC_OFFS_0");
h->acc_offs_1 = param_find("INAV_ACC_OFFS_1");
h->acc_offs_2 = param_find("INAV_ACC_OFFS_2");
@@ -101,6 +106,8 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->k_alt_20, &(p->k[2][0]));
param_get(h->k_alt_21, &(p->k[2][1]));
param_get(h->acc_offs_w, &(p->acc_offs_w));
/* read int32 and cast to int16 */
int32_t t;
param_get(h->acc_offs_0, &t);
@@ -119,5 +126,6 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->acc_t_20, &(p->acc_T[2][0]));
param_get(h->acc_t_21, &(p->acc_T[2][1]));
param_get(h->acc_t_22, &(p->acc_T[2][2]));
return OK;
}
apps/position_estimator_inav/position_estimator_inav_params.h
+3
View File
@@ -43,6 +43,7 @@
struct position_estimator_inav_params {
int use_gps;
float k[3][2];
float acc_offs_w;
int16_t acc_offs[3];
float acc_T[3][3];
};
@@ -57,6 +58,8 @@ struct position_estimator_inav_param_handles {
param_t k_alt_20;
param_t k_alt_21;
param_t acc_offs_w;
param_t acc_offs_0;
param_t acc_offs_1;
param_t acc_offs_2;