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

Use GPS velocity in position estimator

Browse Source
This commit is contained in:
Anton Babushkin
2013-05-20 19:47:38 +04:00
parent e8ec2ed93a
commit 5842c22123
5 changed files with 46 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/kalman_filter_inertial.c
+16 -1
View File
@@ -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];
}
}
}
src/modules/position_estimator_inav/kalman_filter_inertial.h
+3 -1
View File
@@ -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]);
src/modules/position_estimator_inav/position_estimator_inav_main.c
+14 -11
View File
@@ -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;
}
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]) {
if (accelerometer_updated) {
x_meas[2] = accel_NED[0];
y_meas[2] = accel_NED[1];
use_xy[2] = true;
}
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);
}
}
}
src/modules/position_estimator_inav/position_estimator_inav_params.c
+9
View File
@@ -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;
}
src/modules/position_estimator_inav/position_estimator_inav_params.h
+4 -1
View File
@@ -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;
};
/**