示例#1
0
LQRController::UpdateGainsJob::UpdateGainsJob(LQRController * c, const StateMatrix & weights, const InputMatrix &input_weights, const StateVector & target)
: c_(c), target_(target)
{
  if(!c)
    return;
  ROS_DEBUG_STREAM("target is "<<target.transpose());
  const int n=weights.rows();
  const int m=input_weights.rows();
  
  input_offset_.resize(m,1);
  gains_.resize(n,n);
  
  //Computes a linearization around the target point
  StateMatrix A(n,n);
  InputMatrix B(n,m);
  if(!c_->model_->getLinearization(target, A, B, input_offset_))
  {
    ROS_ERROR("Failed to get linearization");
    c_=NULL;
    return;
  }
  ROS_DEBUG_STREAM("************ Obtained linearization **********\n[A]\n"<<A<<std::endl<<"[B]"<<std::endl<<B<<std::endl<<"[input offset]"<<std::endl<<input_offset_<<"\n*************");
  ROS_DEBUG("Sanity checks");
  ROS_ASSERT(LQR::isCommandable(A,B));
  // Compute new gains matrix
  ROS_DEBUG("Computing LQR gains matrix. Hold on to your seatbelts...");
  LQR::LQRDP<StateMatrix,InputMatrix,StateMatrix,InputMatrix>::runContinuous(A, B, weights, weights, input_weights, 0.01, 0.1, gains_);
  ROS_DEBUG_STREAM("Done. Computed gains:\n***********\n"<<gains_<<"\n**********\n");
  // Check validity
  ROS_DEBUG("Checking gains matrix...");
  bool test=LQR::isConverging(A,B,gains_);
  ROS_ASSERT(test);
  ROS_DEBUG("Valid gains matrix");
//   assert(test);
}
double PerformanceEvaluator::CalculateNEES(SVref xEst,SCMref P,SVref xReal) {
  StateVector x = xReal - xEst;
  double NEES = x.transpose()*P.inverse()*x;
  return NEES;
}