ABCEquivariantFilter.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/VectorConstants.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/navigation/EquivariantFilter.h>
#include <gtsam_unstable/geometry/ABC.h>
 
namespace gtsam {
namespace abc {

template <size_t N>
class AbcEquivariantFilter
    : public gtsam::EquivariantFilter<State<N>, Symmetry<N>> {
 public:
  using gtsam::EquivariantFilter<State<N>, Symmetry<N>>::update;

  AbcEquivariantFilter()
      : AbcEquivariantFilter(Matrix::Identity(6 + 3 * N, 6 + 3 * N)) {}

  explicit AbcEquivariantFilter(const Matrix& Sigma0)
      : gtsam::EquivariantFilter<State<N>, Symmetry<N>>(State<N>::identity(),
                                                        Sigma0) {}

  void predict(const Vector3& omega, const Matrix6& inputCovariance,
               double dt) {
    const Matrix Q = inputProcessNoise<N>(inputCovariance);
    const Vector6 u = toInputVector(omega);
    const Lift<N> lift_u(u);
    const typename InputAction<N>::Orbit psi_u(u);
 
    const Group<N> X_hat = this->groupEstimate();
    const Matrix A = stateMatrixA<N>(psi_u, X_hat);
    const Matrix B = inputMatrixB<N>(X_hat);
    const Matrix Qc = B * Q * B.transpose();
 
    this->template predictWithJacobian<2>(lift_u, A, Qc, dt);
  }

  void update(const Unit3& y, const Unit3& d, const Matrix3& R, int cal_idx) {
    const Innovation<N> innovation(y, d, cal_idx);
    const Group<N> X_hat = this->groupEstimate();
    const Matrix3 D = outputMatrixD<N>(X_hat, cal_idx);
    const Matrix3 R_adjusted = D * R * D.transpose();
    this->template update<Vector3>(innovation, Z_3x1, R_adjusted);
  }

  Rot3 attitude() const { return this->state().R; }

  Vector3 bias() const { return this->state().b; }

  Rot3 calibration(size_t i) const { return this->state().S[i]; }
};
 
}  // namespace abc
}  // namespace gtsam