Function lupnt::RunBatchFilter

• Defined in File batch_filter.cc

Function Documentation

BatchFilterResults lupnt::RunBatchFilter(const VecXd &initial_state, const VecXd &initial_covariance, const std::vector<VecXd> &measurements, const std::vector<VecXd> &measurement_weights, MeasurementModelFunction measurement_model, const BatchFilterConfig &config, const VecXd &true_state = VecXd())

Generic batch filter using iterative weighted least squares.

Solves for state that best fits all measurements using iterative nonlinear least squares. Works with any measurement model provided via function pointer.

Parameters:

• initial_state – Initial state estimate [n_state]

• initial_covariance – Initial state covariance diagonal [n_state] (for initialization constraints)

• measurements – Vector of measurements [n_measurements] each of size [n_meas_per_obs]

• measurement_weights – Vector of measurement weights [n_measurements] each of size [n_meas_per_obs] (1/sigma^2)

• measurement_model – Function that predicts measurements and computes Jacobians

• config – Batch filter configuration

• true_state – Optional true state for error computation [n_state]

Returns:

BatchFilterResults