float KalmanFilter::Filter(const float data)
{
PredictState();
PredictCovariance();
Gain();
UpdateState(data);
UpdateCovariance();
return m_x;
}
void ribi::kalman::SteadyStateKalmanFilter::SupplyMeasurementAndInput(
const boost::numeric::ublas::vector<double>& measurements,
const boost::numeric::ublas::vector<double>& input)
{
assert(measurements.size() == input.size());
//Store the calculation
m_last_calculation->Clear();
assert(!m_last_calculation->IsComplete());
m_last_calculation->SetPreviousStateEstimate(this->GetStateEstimate());
//m_last_calculation->SetMeasurement must be set before calling PredictState!
m_last_calculation->SetMeasurement(measurements);
// 1/1) State prediction
const boost::numeric::ublas::vector<double> state_prediction = PredictState(input);
m_state_estimate = state_prediction;
//Store the calculation
m_last_calculation->SetPredictedState(state_prediction);
m_last_calculation->SetUpdatedState(GetStateEstimate());
assert(m_last_calculation->IsComplete());
}
