24 lines
624 B
C++
24 lines
624 B
C++
|
//
|
||
|
|
// 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_;
|
||
|
|
}
|