//
// Created by SJQ on 2023/12/29.
//

#ifndef WHEEL_LEGGED_LQR_OLD_H
#define WHEEL_LEGGED_LQR_OLD_H
#include "user_lib.h"
typedef struct
{
    float* state_vector;
    float* control_vector;

    float* state_cmd;

    int state_num;    //状态量维数
    int control_num;  //控制量维数

    mat control_matrix;  //其实是向量 定义成矩阵方便计算  @todo：更新DSP库 新版本库支持矩阵直接乘数组
    mat state_matrix;
    mat state_cmd_matrix;

    mat state_err;
    mat K_matrix;
}LQRInstance;

void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt);
void LQR_update(LQRInstance* lqrInstance,float* new_state,float* target_state);
void LQR_set_K(LQRInstance* lqrInstance, float *K_value);

#endif //WHEEL_LEGGED_LQR_OLD_H