LQRController.hpp

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Dense>
 
#include <controlpp/math.hpp>
#include <controlpp/ContinuousStateSpace.hpp>
 
 
namespace controlpp{
 
    template<class T, int NStates, int NInputs>
    Eigen::Matrix<T, NInputs, NStates> lqr_continuous(
        const Eigen::Matrix<T, NStates, NStates>& A,
        const Eigen::Matrix<T, NStates, NInputs>& B,
        const Eigen::Matrix<T, NStates, NStates>& Q = Eigen::Matrix<T, NStates, NStates>::Identity(),
        const Eigen::Matrix<T, NInputs, NInputs>& R = Eigen::Matrix<T, NInputs, NInputs>::Identity()
    ){
        const Eigen::Matrix<T, NStates, NStates> P = care_solver(A, B, Q, R);
        const Eigen::Matrix<T, NInputs, NStates> K = R.llt().solve(B.transpose() * P);
        return K;
    }
 
    template<class T, int NStates, int NInputs>
    Eigen::Matrix<T, NInputs, NStates> lqr_discrete(
        const Eigen::Matrix<T, NStates, NStates>& A,
        const Eigen::Matrix<T, NStates, NInputs>& B,
        const Eigen::Matrix<T, NStates, NStates>& Q = Eigen::Matrix<T, NStates, NStates>::Identity(),
        const Eigen::Matrix<T, NInputs, NInputs>& R = Eigen::Matrix<T, NInputs, NInputs>::Identity()
    ){
        const Eigen::Matrix<T, NStates, NStates> P = dare_solver(A, B, Q, R);
        const Eigen::Matrix<T, NInputs, NInputs> M1 = R + B.transpose() * P * B;
        const Eigen::Matrix<T, NInputs, NStates> M2 = B.transpose() * P * A;
        const Eigen::Matrix<T, NInputs, NStates> K = M1.ldlt().solve(M2);
        return K;
    }
 
    template<class T, int NStates, int NInputs, int NOutputs>
    Eigen::Matrix<T, NInputs, NStates> lqr(
        const DiscreteStateSpace<T, NStates, NInputs, NOutputs> Gss,
        const Eigen::Matrix<T, NStates, NStates>& Q,
        const Eigen::Matrix<T, NInputs, NInputs>& R
    ){
        return lqr_discrete(Gss.A(), Gss.B(), Q, R);
    }
 
    template<
        class T, int NStates, int NInputs, int NOutputs, 
        std::convertible_to<T> U1 = T, std::convertible_to<T> U2 = T>
    Eigen::Matrix<T, NInputs, NStates> lqr(
        const DiscreteStateSpace<T, NStates, NInputs, NOutputs> Gss,
        const U1& r = static_cast<U1>(1),
        const U2& eps = static_cast<U2>(0.001)
    ){
        const Eigen::Matrix<T, NStates, NStates> Iq = Eigen::Matrix<T, NStates, NStates>::Identity(); 
        const Eigen::Matrix<T, NStates, NStates> Q1 = Gss.C().transpose() * Gss.C();
        const Eigen::Matrix<T, NStates, NStates> Q = Q1 + Iq * (Q1.norm() * (eps * eps));
        const Eigen::Matrix<T, NInputs, NInputs> R = Eigen::Matrix<T, NInputs, NInputs>::Ones() * (r * r);
        return lqr_discrete(Gss.A(), Gss.B(), Q, R);
    }
 
    template<class T, int NStates, int NInputs, int NOutputs, std::convertible_to<T> U1 = T, std::convertible_to<T> U2 = T>
    Eigen::Matrix<T, NInputs, NStates> lqr_bryson(
        const DiscreteStateSpace<T, NStates, NInputs, NOutputs> Gss,
        const Eigen::Vector<T, NOutputs>& x_max,
        const Eigen::Vector<T, NInputs>& u_max
    ){
        
        const Eigen::Vector<T, NOutputs> q = static_cast<T>(1) / x_max.array().square();
        const Eigen::Matrix<T, NStates, NStates> Iq = Eigen::Matrix<T, NStates, NStates>::Identity(); 
        const Eigen::Matrix<T, NStates, NStates> Q1 = Gss.C().transpose() * q.asDiagonal() * Gss.C();
        const Eigen::Matrix<T, NStates, NStates> Q = Q1 + Iq * (Q1.norm() * 0.000001);
 
        const Eigen::Vector<T, NOutputs> r = static_cast<T>(1) / u_max.array().square();
        const Eigen::Matrix<T, NInputs, NInputs> R = r.asDiagonal();
        
        return lqr_discrete(Gss.A(), Gss.B(), Q, R);
    }
 
    template<class T, int NStates, int NInputs, int NOutputs>
    Eigen::Matrix<T, NInputs, NOutputs> lqr_feed_forward(
        const DiscreteStateSpace<T, NStates, NInputs, NOutputs> Gss,
        Eigen::Matrix<T, NInputs, NStates> LQR
    ){
        // LQR feed forward
        const Eigen::Matrix<double, NStates, NStates> I = controlpp::identity_like(Gss.A());
        const Eigen::Matrix<double, NStates, NStates> M1 = I - Gss.A() + Gss.B() * LQR;
        const Eigen::Matrix<double, 1, 1> M = Gss.C() * M1.partialPivLu().solve(Gss.B());
        const Eigen::Matrix<T, NInputs, NOutputs> F = M.inverse().eval();
        return F;
    }
}