dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #45 from CarlOlsson/move_fuse_function_to_helper 

moved fuse function to ekf_helper.cpp
This commit is contained in:
Roman Bapst 2016-02-13 08:21:28 +01:00
parent 0a9f7e58db 1e766c7510
commit d21ce70eeb
2 changed files with 38 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
EKF/ekf_helper.cpp
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;
}
}

36
EKF/vel_pos_fusion.cpp
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;
}
}