| 16 | } |
| 17 | |
| 18 | float SimpleKalmanFilter::updateEstimate(float mea) |
| 19 | { |
| 20 | _kalman_gain = _err_estimate / (_err_estimate + _err_measure); |
| 21 | _current_estimate = _last_estimate + _kalman_gain * (mea - _last_estimate); |
| 22 | _err_estimate = (1.0f - _kalman_gain) * _err_estimate + fabsf(_last_estimate - _current_estimate) * _q; |
| 23 | _last_estimate = _current_estimate; |
| 24 | |
| 25 | return _current_estimate; |
| 26 | } |
| 27 | |
| 28 | void SimpleKalmanFilter::setMeasurementError(float mea_e) |
| 29 | { |
nothing calls this directly
no outgoing calls
no test coverage detected