IEKF_NavstateExample.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
#include <gtsam/base/Matrix.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/navigation/InvariantEKF.h>
#include <gtsam/navigation/NavState.h>
 
#include <iostream>
 
using namespace std;
using namespace gtsam;
 
Vector9 dynamics(const Vector6& imu) {
  auto a = imu.head<3>();
  auto w = imu.tail<3>();
  Vector9 xi;
  xi << w, Vector3::Zero(), a;
  return xi;
}
 
Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
  return X.position(H);
}
 
int main() {
  // Initial state & covariances
  NavState X0;  // R=I, v=0, t=0
  Matrix9 P0 = Matrix9::Identity() * 0.1;
  InvariantEKF<NavState> ekf(X0, P0);
 
  // Noise & timestep
  double dt = 1.0;
  Matrix9 Q = Matrix9::Identity() * 0.01;
  Matrix3 R = Matrix3::Identity() * 0.5;
 
  // Two IMU samples [a; ω]
  Vector6 imu1;
  imu1 << 0.1, 0, 0, 0, 0.2, 0;
  Vector6 imu2;
  imu2 << 0, 0.3, 0, 0.4, 0, 0;
 
  // Two GPS fixes
  Vector3 z1;
  z1 << 0.3, 0, 0;
  Vector3 z2;
  z2 << 0.6, 0, 0;
 
  cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance()
    << "\n\n";
 
  // --- first predict/update ---
  ekf.predict(dynamics(imu1), dt, Q);
  cout << "--- After predict 1 ---\nX: " << ekf.state()
    << "\nP: " << ekf.covariance() << "\n\n";
  ekf.update(h_gps, z1, R);
  cout << "--- After update 1 ---\nX: " << ekf.state()
    << "\nP: " << ekf.covariance() << "\n\n";
 
  // --- second predict/update ---
  ekf.predict(dynamics(imu2), dt, Q);
  cout << "--- After predict 2 ---\nX: " << ekf.state()
    << "\nP: " << ekf.covariance() << "\n\n";
  ekf.update(h_gps, z2, R);
  cout << "--- After update 2 ---\nX: " << ekf.state()
    << "\nP: " << ekf.covariance() << "\n";
 
  return 0;
}