StateEstimator.h

#ifndef STATE_ESTIMATOR_H
#define STATE_ESTIMATOR_H

#include "Sensor.h"
#include <Klampt/Control/Command.h>
#include <Klampt/Modeling/Robot.h>
#include <KrisLibrary/robotics/Wrench.h>

class ODERobot;

struct RobotStateEstimator
{
  RobotStateEstimator(Robot& _robot) :robot(_robot) {}
  virtual ~RobotStateEstimator() {}
  virtual void ReadSensors(RobotSensors& sensors) {}
  virtual void UpdateModel() {}
  virtual void ReadCommand(const RobotMotorCommand& command) {}
  virtual void Advance(Real dt) {}
  virtual void Reset() {}

  Robot& robot;
};

struct OmniscientStateEstimator : public RobotStateEstimator
{
  OmniscientStateEstimator(Robot& _robot,ODERobot& _oderobot)
    :RobotStateEstimator(_robot),oderobot(_oderobot) {}
  virtual ~OmniscientStateEstimator();
  virtual void UpdateModel();

  ODERobot& oderobot;
};

struct IntegratedStateEstimator : public RobotStateEstimator
{
  IntegratedStateEstimator(Robot& _robot);
  virtual ~IntegratedStateEstimator() {}
  virtual void ReadSensors(RobotSensors& sensors);
  virtual void UpdateModel() {
    robot.UpdateConfig(q_predicted);
    robot.dq = dq_predicted;
  }
  virtual void ReadCommand(const RobotMotorCommand& command);
  virtual void Advance(Real dt);
  virtual void Reset();

  //instead of ReadCommand, this estimator uses SetDDQ
  void SetDDQ(const Vector& ddq) { ddq_predicted = ddq; }

  Real last_dt;
  Vector q_predicted,dq_predicted,ddq_predicted;
  Vector q_sensed,dq_sensed;
  //these correspond to the world positions of the accelerometers in the sensors
  vector<RigidTransform> accelerometerFrames;
  vector<RigidBodyVelocity> accelerometerVels;
};

#endif