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

Position estimation using accel+GPS implemented

Browse Source
This commit is contained in:
Anton Babushkin
2013-05-02 19:02:32 +04:00
parent 2b7e79cd21
commit 861a21ef75
3 changed files with 74 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/position_estimator_inav/position_estimator_inav_main.c
+38 -13
View File
@@ -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);
}
}
}
apps/position_estimator_inav/position_estimator_inav_params.c
+27 -8
View File
@@ -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;
}
apps/position_estimator_inav/position_estimator_inav_params.h
+9 -1
View File
@@ -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;
};
/**