Function lupnt::AddInitializationConstraints

• Defined in File batch_filter.cc

Function Documentation

std::tuple<MatXd, VecXd, VecXd> lupnt::AddInitializationConstraints(const MatXd &H_meas, const VecXd &residuals_meas, const VecXd &weights_meas, const VecXd &initial_state, const VecXd &current_state, const VecXd &init_weights)

Add initialization constraints to weighted least squares problem.

Adds soft constraints to keep state estimate near initial guess

Parameters:

• H_meas – Measurement design matrix [n_meas x n_state]

• residuals_meas – Measurement residuals [n_meas]

• weights_meas – Measurement weights [n_meas]

• initial_state – Initial state estimate [n_state]

• current_state – Current state estimate [n_state]

• init_weights – Initialization weights [n_state] (1/sigma^2)

Returns:

std::tuple<MatXd, VecXd, VecXd> (H_augmented, residuals_augmented, weights_augmented)