|
|
@ -1606,10 +1606,10 @@ void Ekf::calcExtVisRotMat() |
|
|
|
|
|
|
|
|
|
|
|
// apply an input limiter to protect from spikes
|
|
|
|
// apply an input limiter to protect from spikes
|
|
|
|
Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt; |
|
|
|
Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt; |
|
|
|
float input_delta_mag = _input_delta_vec.norm(); |
|
|
|
float input_delta_len = _input_delta_vec.norm(); |
|
|
|
|
|
|
|
|
|
|
|
if (input_delta_mag > 0.1f) { |
|
|
|
if (input_delta_len > 0.1f) { |
|
|
|
rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_mag); |
|
|
|
rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_len); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// Apply a first order IIR low pass filter
|
|
|
|
// Apply a first order IIR low pass filter
|
|
|
|