/*
 * LQR控制器配置参数示例
 * 基于MATLAB仿真get_k.m和get_k_length.m的结果
 */

#include "lqr.h"

/* 
 * LQR增益矩阵多项式拟合系数
 * 从MATLAB仿真get_k.m得到，对应不同腿长下的最优LQR增益
 * 
 * 多项式形式: K(L) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3]
 * 其中 L 为腿长，范围 [0.1, 0.4] 米
 * 
 * 状态向量: [theta, d_theta, x, d_x, phi, d_phi]^T
 * 控制输入: [T, Tp]^T (轮毂力矩, 髋关节力矩)
 */

/* 
 * 根据MATLAB仿真结果设置的增益矩阵
 * Q权重矩阵: diag([100 1 500 100 5000 1]) (theta d_theta x d_x phi d_phi)
 * R权重矩阵: diag([240, 25]) (T Tp)
 */
LQR_GainMatrix_t example_lqr_gains = {
    /* K矩阵第一行 - 轮毂力矩T的增益系数 */
    .k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f},      /* K(1,1): theta */
    .k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f},            /* K(1,2): d_theta */
    .k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f},       /* K(1,3): x */
    .k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f},          /* K(1,4): d_x */
    .k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f},   /* K(1,5): phi */
    .k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f},            /* K(1,6): d_phi */
    
    /* K矩阵第二行 - 髋关节力矩Tp的增益系数 */
    .k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f},          /* K(2,1): theta */
    .k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f},             /* K(2,2): d_theta */
    .k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f},         /* K(2,3): x */
    .k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f},            /* K(2,4): d_x */
    .k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f},       /* K(2,5): phi */
    .k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f},             /* K(2,6): d_phi */
};

/* 
 * 使用示例：
 * 
 * 1. 在底盘参数结构体中设置LQR增益：
 * Chassis_Params_t chassis_params = {
 *     .lqr_param = {
 *         .max_wheel_torque = 10.0f,  // 10 N·m
 *         .max_joint_torque = 5.0f,   // 5 N·m
 *     },
 *     .lqr_gains = example_lqr_gains,
 *     // ... 其他参数
 * };
 * 
 * 2. 初始化底盘时会自动设置LQR增益矩阵
 * 
 * 3. 在控制循环中，LQR控制器会根据当前腿长自动计算增益矩阵
 */

/*
 * 调试信息：
 * 
 * 可以通过以下方式验证增益矩阵计算是否正确：
 * 
 * 对于腿长 L = 0.25m：
 * K(1,1) = -2845.3*0.25^2 + 1899.4*0.25 - 123.8 ≈ -123.8
 * K(1,2) = -89.7*0.25^2 + 61.2*0.25 - 4.8 ≈ -4.8
 * ... 其他系数类似计算
 * 
 * 这些数值应该与MATLAB仿真中对应腿长的LQR增益一致
 */