wheel_legged/modules/algorithm/LQR.h

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

#ifndef WHEEL_LEGGED_LQR_H
#define WHEEL_LEGGED_LQR_H

#include <matrix.h>
template <int _state,int _control>
class LQR {
public:
    LQR(){};
    void update(float* new_state,float* target_state);
    void set_k(float *K_value);
    Matrixf<_control,1> get_control();
private:
    Matrixf<_state,1> state_vec_;

    Matrixf<_state,1> state_cmd_;
    Matrixf<_control,1> control_vec_;

    Matrixf<_control,_state> K_;
};


#endif //WHEEL_LEGGED_LQR_H