//
// Created by SJQ on 2024/3/2.
//

#include "LQR.h"

template <int _state,int _control>
void LQR<_state,_control>::set_k(float *K_value)
{
    K_ = Matrixf<_state,_control>((float[_state * _control])K_value);
}
template <int _state,int _control>
void LQR<_state,_control>::update(float* new_state,float* target_state)
{
    state_vec_ = Matrixf<_state,1>((float[_state])new_state);
    state_cmd_ = Matrixf<_state,1>((float[_state])target_state);

    control_vec_ =  K_ * (state_cmd_ - state_vec_);
}
template <int _state,int _control>
Matrixf<_control,1> LQR<_state,_control>::get_control()
{
    return control_vec_;
}