forked from Archive/PX4-Autopilot
Remove direct unsafe access to matrix internal data
This commit is contained in:
parent
c446ee444a
commit
8d60f8ba8f
11
EKF/ekf.h
11
EKF/ekf.h
|
@ -227,21 +227,26 @@ public:
|
|||
// return the amount the local horizontal position changed in the last reset and the number of reset events
|
||||
void get_posNE_reset(float delta[2], uint8_t *counter)
|
||||
{
|
||||
memcpy(delta, &_state_reset_status.posNE_change._data[0], sizeof(_state_reset_status.posNE_change._data));
|
||||
delta[0] = _state_reset_status.posNE_change(0);
|
||||
delta[1] = _state_reset_status.posNE_change(1);
|
||||
*counter = _state_reset_status.posNE_counter;
|
||||
}
|
||||
|
||||
// return the amount the local horizontal velocity changed in the last reset and the number of reset events
|
||||
void get_velNE_reset(float delta[2], uint8_t *counter)
|
||||
{
|
||||
memcpy(delta, &_state_reset_status.velNE_change._data[0], sizeof(_state_reset_status.velNE_change._data));
|
||||
delta[0] = _state_reset_status.velNE_change(0);
|
||||
delta[1] = _state_reset_status.velNE_change(1);
|
||||
*counter = _state_reset_status.velNE_counter;
|
||||
}
|
||||
|
||||
// return the amount the quaternion has changed in the last reset and the number of reset events
|
||||
void get_quat_reset(float delta_quat[4], uint8_t *counter)
|
||||
{
|
||||
memcpy(delta_quat, &_state_reset_status.quat_change._data[0], sizeof(_state_reset_status.quat_change._data));
|
||||
for (size_t i = 0; i < 4; i++)
|
||||
{
|
||||
delta_quat[i] = _state_reset_status.quat_change(i);
|
||||
}
|
||||
*counter = _state_reset_status.quat_counter;
|
||||
}
|
||||
|
||||
|
|
|
@ -277,13 +277,6 @@ public:
|
|||
// return true if the local position estimate is valid
|
||||
bool local_position_is_valid();
|
||||
|
||||
void copy_quaternion(float *quat)
|
||||
{
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
quat[i] = _output_new.quat_nominal(i);
|
||||
}
|
||||
}
|
||||
|
||||
const matrix::Quatf &get_quaternion() const { return _output_new.quat_nominal; }
|
||||
|
||||
// return the quaternion defining the rotation from the EKF to the External Vision reference frame
|
||||
|
|
|
@ -175,7 +175,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
|
|||
|
||||
const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;
|
||||
|
||||
K /= (S._data[0][0]);
|
||||
K /= S(0,0);
|
||||
// compute innovation
|
||||
const float airspeed_pred = _state(tas) * sqrtf((v_n - _state(w_n)) * (v_n - _state(w_n)) + (v_e - _state(w_e)) *
|
||||
(v_e - _state(w_e)) + v_d * v_d);
|
||||
|
@ -183,7 +183,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
|
|||
_tas_innov = true_airspeed - airspeed_pred;
|
||||
|
||||
// innovation variance
|
||||
_tas_innov_var = S._data[0][0];
|
||||
_tas_innov_var = S(0,0);
|
||||
|
||||
bool reinit_filter = false;
|
||||
bool meas_is_rejected = false;
|
||||
|
@ -261,7 +261,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
|
|||
|
||||
const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var;
|
||||
|
||||
K /= (S._data[0][0]);
|
||||
K /= S(0,0);
|
||||
|
||||
// compute predicted side slip angle
|
||||
matrix::Vector3f rel_wind(velI(0) - _state(w_n), velI(1) - _state(w_e), velI(2));
|
||||
|
@ -276,7 +276,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
|
|||
const float beta_pred = rel_wind(1) / rel_wind(0);
|
||||
|
||||
_beta_innov = 0.0f - beta_pred;
|
||||
_beta_innov_var = S._data[0][0];
|
||||
_beta_innov_var = S(0,0);
|
||||
|
||||
bool reinit_filter = false;
|
||||
bool meas_is_rejected = false;
|
||||
|
|
Loading…
Reference in New Issue