ukf-nonlinear-complex-example.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

 
#include <iostream>
 
// UKF includes
#include <visp3/core/vpUKSigmaDrawerMerwe.h>
#include <visp3/core/vpUnscentedKalman.h>
 
// ViSP includes
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpGaussRand.h>
#ifdef VISP_HAVE_DISPLAY
#include <visp3/gui/vpPlot.h>
#endif
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
 
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
 
namespace
{
double normalizeAngle(const double &angle)
{
  double angleIn0to2pi = vpMath::modulo(angle, 2. * M_PI);
  double angleInMinPiPi = angleIn0to2pi;
  if (angleInMinPiPi > M_PI) {
    // Substract 2 PI to be in interval [-Pi; Pi]
    angleInMinPiPi -= 2. * M_PI;
  }
  return angleInMinPiPi;
}

vpColVector measurementMean(const std::vector<vpColVector> &measurements, const std::vector<double> &wm)
{
  const unsigned int nbPoints = static_cast<unsigned int>(measurements.size());
  const unsigned int sizeMeasurement = measurements[0].size();
  const unsigned int nbLandmarks = sizeMeasurement / 2;
  vpColVector mean(sizeMeasurement, 0.);
  std::vector<double> sumCos(nbLandmarks, 0.);
  std::vector<double> sumSin(nbLandmarks, 0.);
  for (unsigned int i = 0; i < nbPoints; ++i) {
    for (unsigned int j = 0; j < nbLandmarks; ++j) {
      mean[2*j] += wm[i] * measurements[i][2*j];
      sumCos[j] += wm[i] * std::cos(measurements[i][(2*j)+1]);
      sumSin[j] += wm[i] * std::sin(measurements[i][(2*j)+1]);
    }
  }
  for (unsigned int j = 0; j < nbLandmarks; ++j) {
    mean[(2*j)+1] = std::atan2(sumSin[j], sumCos[j]);
  }
  return mean;
}

vpColVector measurementResidual(const vpColVector &meas, const vpColVector &toSubtract)
{
  vpColVector res = meas - toSubtract;
  unsigned int nbMeasures = res.size();
  for (unsigned int i = 1; i < nbMeasures; i += 2) {
    res[i] = normalizeAngle(res[i]);
  }
  return res;
}

vpColVector stateAdd(const vpColVector &state, const vpColVector &toAdd)
{
  vpColVector add = state + toAdd;
  add[2] = normalizeAngle(add[2]);
  return add;
}

vpColVector stateMean(const std::vector<vpColVector> &states, const std::vector<double> &wm)
{
  vpColVector mean(3, 0.);
  unsigned int nbPoints = static_cast<unsigned int>(states.size());
  double sumCos = 0.;
  double sumSin = 0.;
  for (unsigned int i = 0; i < nbPoints; ++i) {
    mean[0] += wm[i] * states[i][0];
    mean[1] += wm[i] * states[i][1];
    sumCos += wm[i] * std::cos(states[i][2]);
    sumSin += wm[i] * std::sin(states[i][2]);
  }
  mean[2] = std::atan2(sumSin, sumCos);
  return mean;
}

vpColVector stateResidual(const vpColVector &state, const vpColVector &toSubtract)
{
  vpColVector res = state - toSubtract;
  res[2] = normalizeAngle(res[2]);
  return res;
}

vpColVector fx(const vpColVector &x, const double & /*dt*/)
{
  return x;
}

std::vector<vpColVector> generateTurnCommands(const double &v, const double &angleStart, const double &angleStop, const unsigned int &nbSteps)
{
  std::vector<vpColVector> cmds;
  double dTheta = vpMath::rad(angleStop - angleStart) / static_cast<double>(nbSteps - 1);
  for (unsigned int i = 0; i < nbSteps; ++i) {
    double theta = vpMath::rad(angleStart) + dTheta * static_cast<double>(i);
    vpColVector cmd(2);
    cmd[0] = v;
    cmd[1] = theta;
    cmds.push_back(cmd);
  }
  return cmds;
}

std::vector<vpColVector> generateCommands()
{
  std::vector<vpColVector> cmds;
  // Starting by an straight line acceleration
  unsigned int nbSteps = 30;
  double dv = (1.1 - 0.001) / static_cast<double>(nbSteps - 1);
  for (unsigned int i = 0; i < nbSteps; ++i) {
    vpColVector cmd(2);
    cmd[0] = 0.001 + static_cast<double>(i) * dv;
    cmd[1] = 0.;
    cmds.push_back(cmd);
  }
 
  // Left turn
  double lastLinearVelocity = cmds[cmds.size() -1][0];
  std::vector<vpColVector> leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 2, 15);
  cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
  for (unsigned int i = 0; i < 100; ++i) {
    cmds.push_back(cmds[cmds.size() -1]);
  }
 
  // Right turn
  lastLinearVelocity = cmds[cmds.size() -1][0];
  std::vector<vpColVector> rightTurnCmds = generateTurnCommands(lastLinearVelocity, 2, -2, 15);
  cmds.insert(cmds.end(), rightTurnCmds.begin(), rightTurnCmds.end());
  for (unsigned int i = 0; i < 200; ++i) {
    cmds.push_back(cmds[cmds.size() -1]);
  }
 
  // Left  turn again
  lastLinearVelocity = cmds[cmds.size() -1][0];
  leftTurnCmds = generateTurnCommands(lastLinearVelocity, -2, 0, 15);
  cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
  for (unsigned int i = 0; i < 150; ++i) {
    cmds.push_back(cmds[cmds.size() -1]);
  }
 
  lastLinearVelocity = cmds[cmds.size() -1][0];
  leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 1, 25);
  cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
  for (unsigned int i = 0; i < 150; ++i) {
    cmds.push_back(cmds[cmds.size() -1]);
  }
 
  return cmds;
}
}

class vpBicycleModel
{
public:
  vpBicycleModel(const double &w)
    : m_w(w)
  { }

  vpColVector computeMotion(const vpColVector &u, const vpColVector &x, const double &dt)
  {
    double heading = x[2];
    double vel = u[0];
    double steeringAngle = u[1];
    double distance = vel * dt;
 
    if (std::abs(steeringAngle) > 0.001) {
      // The robot is turning
      double beta = (distance / m_w) * std::tan(steeringAngle);
      double radius = m_w / std::tan(steeringAngle);
      double sinh = std::sin(heading);
      double sinhb = std::sin(heading + beta);
      double cosh = std::cos(heading);
      double coshb = std::cos(heading + beta);
      vpColVector motion(3);
      motion[0] = -radius * sinh + radius * sinhb;
      motion[1] = radius * cosh - radius * coshb;
      motion[2] = beta;
      return motion;
    }
    else {
      // The robot is moving in straight line
      vpColVector motion(3);
      motion[0] = distance * std::cos(heading);
      motion[1] = distance * std::sin(heading);
      motion[2] = 0.;
      return motion;
    }
  }

  vpColVector move(const vpColVector &u, const vpColVector &x, const double &dt)
  {
    vpColVector motion = computeMotion(u, x, dt);
    vpColVector newX = x + motion;
    newX[2] = normalizeAngle(newX[2]);
    return newX;
  }
private:
  double m_w; // The length of the wheelbase.
};

class vpLandmarkMeasurements
{
public:
  vpLandmarkMeasurements(const double &x, const double &y, const double &range_std, const double &rel_angle_std)
    : m_x(x)
    , m_y(y)
    , m_rngRange(range_std, 0., 4224)
    , m_rngRelativeAngle(rel_angle_std, 0., 2112)
  { }

  vpColVector state_to_measurement(const vpColVector &chi)
  {
    vpColVector meas(2);
    double dx = m_x - chi[0];
    double dy = m_y - chi[1];
    meas[0] = std::sqrt(dx * dx + dy * dy);
    meas[1] = normalizeAngle(std::atan2(dy, dx) - chi[2]);
    return meas;
  }

  vpColVector measureGT(const vpColVector &pos)
  {
    double dx = m_x - pos[0];
    double dy = m_y - pos[1];
    double range = std::sqrt(dx * dx + dy * dy);
    double orientation = normalizeAngle(std::atan2(dy, dx) - pos[2]);
    vpColVector measurements(2);
    measurements[0] = range;
    measurements[1] = orientation;
    return measurements;
  }

  vpColVector measureWithNoise(const vpColVector &pos)
  {
    vpColVector measurementsGT = measureGT(pos);
    vpColVector measurementsNoisy = measurementsGT;
    measurementsNoisy[0] += m_rngRange();
    measurementsNoisy[1] += m_rngRelativeAngle();
    measurementsNoisy[1] = normalizeAngle(measurementsNoisy[1]);
    return measurementsNoisy;
  }
 
private:
  double m_x; // The position along the x-axis of the landmark
  double m_y; // The position along the y-axis of the landmark
  vpGaussRand m_rngRange; // Noise simulator for the range measurement
  vpGaussRand m_rngRelativeAngle; // Noise simulator for the relative angle measurement
};

class vpLandmarksGrid
{
public:
  vpLandmarksGrid(const std::vector<vpLandmarkMeasurements> &landmarks)
    : m_landmarks(landmarks)
  { }

  vpColVector state_to_measurement(const vpColVector &chi)
  {
    unsigned int nbLandmarks = static_cast<unsigned int>(m_landmarks.size());
    vpColVector measurements(2*nbLandmarks);
    for (unsigned int i = 0; i < nbLandmarks; ++i) {
      vpColVector landmarkMeas = m_landmarks[i].state_to_measurement(chi);
      measurements[2*i] = landmarkMeas[0];
      measurements[(2*i) + 1] = landmarkMeas[1];
    }
    return measurements;
  }

  vpColVector measureGT(const vpColVector &pos)
  {
    unsigned int nbLandmarks = static_cast<unsigned int>(m_landmarks.size());
    vpColVector measurements(2*nbLandmarks);
    for (unsigned int i = 0; i < nbLandmarks; ++i) {
      vpColVector landmarkMeas = m_landmarks[i].measureGT(pos);
      measurements[2*i] = landmarkMeas[0];
      measurements[(2*i) + 1] = landmarkMeas[1];
    }
    return measurements;
  }

  vpColVector measureWithNoise(const vpColVector &pos)
  {
    unsigned int nbLandmarks = static_cast<unsigned int>(m_landmarks.size());
    vpColVector measurements(2*nbLandmarks);
    for (unsigned int i = 0; i < nbLandmarks; ++i) {
      vpColVector landmarkMeas = m_landmarks[i].measureWithNoise(pos);
      measurements[2*i] = landmarkMeas[0];
      measurements[(2*i) + 1] = landmarkMeas[1];
    }
    return measurements;
  }
 
private:
  std::vector<vpLandmarkMeasurements> m_landmarks; 
};
 
int main(const int argc, const char *argv[])
{
  bool opt_useDisplay = true;
  bool opt_useUserInteraction = true;
  for (int i = 1; i < argc; ++i) {
    std::string arg(argv[i]);
    if (arg == "-d") {
      opt_useDisplay = false;
    }
    if (arg == "-c") {
      opt_useUserInteraction = false;
    }
    else if ((arg == "-h") || (arg == "--help")) {
      std::cout << "SYNOPSIS" << std::endl;
      std::cout << "  " << argv[0] << " [-d][-h]" << std::endl;
      std::cout << std::endl << std::endl;
      std::cout << "DETAILS" << std::endl;
      std::cout << "  -d" << std::endl;
      std::cout << "    Deactivate display." << std::endl;
      std::cout << "  -c" << std::endl;
      std::cout << "    Deactivate user interaction." << std::endl;
      std::cout << std::endl;
      std::cout << "  -h, --help" << std::endl;
      return 0;
    }
  }
 
  const double dt = 0.1; // Period of 0.1s
  const double step = 1.; // Number of update of the robot position between two UKF filtering
  const double sigmaRange = 0.3; // Standard deviation of the range measurement: 0.3m
  const double sigmaBearing = vpMath::rad(0.5); // Standard deviation of the bearing angle: 0.5deg
  const double wheelbase = 0.5; // Wheelbase of 0.5m
  const std::vector<vpLandmarkMeasurements> landmarks = { vpLandmarkMeasurements(5, 10, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(10, 5, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(15, 15, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(20, 5, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(0, 30, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(50, 30, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(40, 10, sigmaRange, sigmaBearing) }; // Vector of landmarks constituting the grid
  const unsigned int nbLandmarks = static_cast<unsigned int>(landmarks.size()); // Number of landmarks constituting the grid
  std::vector<vpColVector> cmds = generateCommands();
  const unsigned int nbCmds = static_cast<unsigned int>(cmds.size());
 
  // Initialize the attributes of the UKF
  std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(3, 0.1, 2., 0, stateResidual, stateAdd);
 
  vpMatrix R1landmark(2, 2, 0.); // The covariance of the noise introduced by the measurement with 1 landmark
  R1landmark[0][0] = sigmaRange*sigmaRange;
  R1landmark[1][1] = sigmaBearing*sigmaBearing;
  vpMatrix R(2*nbLandmarks, 2 * nbLandmarks);
  for (unsigned int i = 0; i < nbLandmarks; ++i) {
    R.insert(R1landmark, 2*i, 2*i);
  }
 
  const double processVariance = 0.0001;
  vpMatrix Q; // The covariance of the process
  Q.eye(3);
  Q = Q * processVariance;
 
  vpMatrix P0(3, 3); //  The initial guess of the process covariance
  P0.eye(3);
  P0[0][0] = 0.1;
  P0[1][1] = 0.1;
  P0[2][2] = 0.05;
 
  vpColVector X0(3);
  X0[0] = 2.; // x = 2m
  X0[1] = 6.; // y = 6m
  X0[2] = 0.3; // robot orientation = 0.3 rad
 
  vpUnscentedKalman::vpProcessFunction f = fx;
  vpLandmarksGrid grid(landmarks);
  vpBicycleModel robot(wheelbase);
  using std::placeholders::_1;
  using std::placeholders::_2;
  using std::placeholders::_3;
  vpUnscentedKalman::vpMeasurementFunction h = std::bind(&vpLandmarksGrid::state_to_measurement, &grid, _1);
  vpUnscentedKalman::vpCommandStateFunction bx = std::bind(&vpBicycleModel::computeMotion, &robot, _1, _2, _3);
 
  // Initialize the UKF
  vpUnscentedKalman ukf(Q, R, drawer, f, h);
  ukf.init(X0, P0);
  ukf.setCommandStateFunction(bx);
  ukf.setMeasurementMeanFunction(measurementMean);
  ukf.setMeasurementResidualFunction(measurementResidual);
  ukf.setStateAddFunction(stateAdd);
  ukf.setStateMeanFunction(stateMean);
  ukf.setStateResidualFunction(stateResidual);
 
#ifdef VISP_HAVE_DISPLAY
  vpPlot *plot = nullptr;
  if (opt_useDisplay) {
  // Initialize the plot
    plot = new vpPlot(1);
    plot->initGraph(0, 2);
    plot->setTitle(0, "Position of the robot");
    plot->setUnitX(0, "Position along x(m)");
    plot->setUnitY(0, "Position along y (m)");
    plot->setLegend(0, 0, "GT");
    plot->setLegend(0, 1, "Filtered");
  }
#else
  (void)opt_useDisplay;
#endif
 
  // Initialize the simulation
  vpColVector robot_pos = X0;
 
  for (unsigned int i = 0; i < nbCmds; ++i) {
    robot_pos = robot.move(cmds[i], robot_pos, dt / step);
    if (i % static_cast<int>(step) == 0) {
      // Perform the measurement
      vpColVector z = grid.measureWithNoise(robot_pos);
 
      // Use the UKF to filter the measurement
      ukf.filter(z, dt, cmds[i]);
 
#ifdef VISP_HAVE_DISPLAY
      if (opt_useDisplay) {
        // Plot the filtered state
        vpColVector Xest = ukf.getXest();
        plot->plot(0, 1, Xest[0], Xest[1]);
      }
#endif
    }
 
#ifdef VISP_HAVE_DISPLAY
    if (opt_useDisplay) {
    // Plot the ground truth
      plot->plot(0, 0, robot_pos[0], robot_pos[1]);
    }
#endif
  }
 
  if (opt_useUserInteraction) {
    std::cout << "Press Enter to quit..." << std::endl;
    std::cin.get();
  }
 
#ifdef VISP_HAVE_DISPLAY
  if (opt_useDisplay) {
    delete plot;
  }
#endif
 
  vpColVector finalError = grid.state_to_measurement(ukf.getXest()) - grid.measureGT(robot_pos);
  const double maxError = 0.3;
  if (finalError.frobeniusNorm() > maxError) {
    std::cerr << "Error: max tolerated error = " << maxError << ", final error = " << finalError.frobeniusNorm() << std::endl;
    return -1;
  }
  return 0;
}
#else
int main()
{
  std::cout << "This example is only available if you compile ViSP in C++11 standard or higher." << std::endl;
  return 0;
}
#endif