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;
}