31 lines
774 B
C
31 lines
774 B
C
|
//
|
|||
|
|
// 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
|