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

moved fuse function to ekf_helper.cpp

Browse Source
This commit is contained in:
CarlOlsson
2016-02-12 15:54:32 +01:00
parent 0a9f7e58db
commit 1e766c7510
2 changed files with 38 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+38
View File
@@ -275,3 +275,41 @@ void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig
memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
}
// fuse measurement
void Ekf::fuse(float *K, float innovation)
{
for (unsigned i = 0; i < 3; i++) {
_state.ang_error(i) = _state.ang_error(i) - K[i] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.vel(i) = _state.vel(i) - K[i + 3] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.pos(i) = _state.pos(i) - K[i + 6] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation;
}
_state.accel_z_bias -= K[15] * innovation;
for (unsigned i = 0; i < 3; i++) {
_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation;
}
for (unsigned i = 0; i < 2; i++) {
_state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation;
}
}
EKF/vel_pos_fusion.cpp
-36
View File
@@ -190,39 +190,3 @@ void Ekf::fuseVelPosHeight()
}
void Ekf::fuse(float *K, float innovation)
{
for (unsigned i = 0; i < 3; i++) {
_state.ang_error(i) = _state.ang_error(i) - K[i] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.vel(i) = _state.vel(i) - K[i + 3] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.pos(i) = _state.pos(i) - K[i + 6] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation;
}
_state.accel_z_bias -= K[15] * innovation;
for (unsigned i = 0; i < 3; i++) {
_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
_state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation;
}
for (unsigned i = 0; i < 2; i++) {
_state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation;
}
}