51 lines
2.1 KiB
C
51 lines
2.1 KiB
C
//
|
|
// Created by SJQ on 2023/12/29.
|
|
//
|
|
|
|
#include "LQR_old.h"
|
|
|
|
void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt)
|
|
{
|
|
lqrInstance->state_num = state_cnt;
|
|
lqrInstance->control_num = control_cnt;
|
|
|
|
lqrInstance->state_vector = (float *)zmalloc(state_cnt *4);
|
|
lqrInstance->state_cmd = (float *)zmalloc(state_cnt *4);
|
|
|
|
lqrInstance->control_vector = (float *)zmalloc(control_cnt *4);
|
|
|
|
MatInit(&lqrInstance->K_matrix,control_cnt,state_cnt);
|
|
MatInit(&lqrInstance->state_err,state_cnt,1);
|
|
MatInit(&lqrInstance->state_matrix,state_cnt,1);
|
|
MatInit(&lqrInstance->control_matrix,control_cnt,1);
|
|
MatInit(&lqrInstance->state_cmd_matrix,state_cnt,1);
|
|
|
|
}
|
|
|
|
void LQR_update(LQRInstance* lqrInstance,float* new_state,float* target_state)
|
|
{
|
|
memcpy(lqrInstance->state_vector,new_state,lqrInstance->state_num * 4);
|
|
memcpy(lqrInstance->state_matrix.pData,new_state,lqrInstance->state_num * 4);
|
|
|
|
memcpy(lqrInstance->state_cmd,target_state,lqrInstance->state_num* 4);
|
|
memcpy(lqrInstance->state_cmd_matrix.pData,target_state,lqrInstance->state_num* 4);
|
|
|
|
arm_mat_sub_f32(&lqrInstance->state_cmd_matrix,&lqrInstance->state_matrix,&lqrInstance->state_err);
|
|
|
|
arm_mat_mult_f32(&lqrInstance->K_matrix,&lqrInstance->state_err,&lqrInstance->control_matrix);
|
|
|
|
memcpy(lqrInstance->control_vector,lqrInstance->control_matrix.pData,lqrInstance->control_num* 4);
|
|
|
|
// arm_mat_init_f32(&lqrInstance->state_matrix,lqrInstance->state_num,1,new_state);
|
|
// arm_mat_init_f32(&lqrInstance->state_cmd_matrix,lqrInstance->state_num,1,target_state);
|
|
//
|
|
// arm_mat_sub_f32(&lqrInstance->state_cmd_matrix,&lqrInstance->state_matrix,&lqrInstance->state_err);
|
|
// arm_mat_mult_f32(&lqrInstance->K_matrix,&lqrInstance->state_err,&lqrInstance->control_matrix);
|
|
}
|
|
|
|
void LQR_set_K(LQRInstance* lqrInstance, float *K_value)
|
|
{
|
|
int K_cnt = lqrInstance->K_matrix.numCols * lqrInstance->K_matrix.numRows;
|
|
memcpy(lqrInstance->K_matrix.pData,K_value,K_cnt *4);
|
|
//arm_mat_init_f32(&lqrInstance->K_matrix,lqrInstance->control_num,lqrInstance->state_num,K_value);
|
|
} |