Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Fix for UnscentedKalmannFilter
Browse files Browse the repository at this point in the history
condor-cp committed Mar 28, 2022
1 parent 7d36e42 commit c9ebddf
Showing 3 changed files with 8 additions and 5 deletions.
2 changes: 1 addition & 1 deletion ct_optcon/examples/KalmanFiltering.cpp
Original file line number Diff line number Diff line change
@@ -105,7 +105,7 @@ int main(int argc, char** argv)
new ct::optcon::CTSystemModel<state_dim, control_dim>(oscillator_observer_model, sensApprox, dFdv));

// set up the measurement model
ct::core::OutputStateMatrix<output_dim, state_dim> dHdw;
ct::core::OutputMatrix<output_dim> dHdw;
dHdw.setIdentity();
std::shared_ptr<ct::optcon::LinearMeasurementModel<output_dim, state_dim>> measModel(
new ct::optcon::LTIMeasurementModel<output_dim, state_dim>(C, dHdw));
Original file line number Diff line number Diff line change
@@ -62,6 +62,7 @@ UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::UnscentedKalm
const ct::core::StateMatrix<STATE_DIM, SCALAR>& P0)
: Base(f, h, x0), alpha_(alpha), beta_(beta), kappa_(kappa), P_(P0)
{
computeWeights();
}

template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR>
@@ -75,6 +76,7 @@ UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::UnscentedKalm
kappa_(ukf_settings.kappa),
P_(ukf_settings.P0)
{
computeWeights();
}

template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR>
@@ -263,8 +265,9 @@ void UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::computeS

template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR>
template <size_t DIM>
auto UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::computePredictionFromSigmaPoints(
const SigmaPoints<DIM>& sigmaPoints) -> state_vector_t
Eigen::Matrix<SCALAR, DIM, 1>
UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::computePredictionFromSigmaPoints(
const SigmaPoints<DIM>& sigmaPoints)
{
// Use efficient matrix x vector computation
return sigmaPoints * sigmaWeights_m_;
4 changes: 2 additions & 2 deletions ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter.h
Original file line number Diff line number Diff line change
@@ -147,9 +147,9 @@ class UnscentedKalmanFilter final : public EstimatorBase<STATE_DIM, CONTROL_DIM,
//! Predict measurements of sigma points.
void computeSigmaPointMeasurements(SigmaPoints<OUTPUT_DIM>& sigmaMeasurementPoints, const ct::core::Time& t = 0);

//! Make a prediction based on sigma points.
//! Make a prediction based on sigma points. Used for both state and output prediction.
template <size_t DIM>
state_vector_t computePredictionFromSigmaPoints(const SigmaPoints<DIM>& sigmaPoints);
Eigen::Matrix<SCALAR, DIM, 1> computePredictionFromSigmaPoints(const SigmaPoints<DIM>& sigmaPoints);

private:
state_matrix_t P_; //! Covariance matrix.

0 comments on commit c9ebddf

Please sign in to comment.