/**********************************
 * 文件名称: L_identification_wrapper.c
 * 功能描述: 电感识别模块
 * 功能: 使用递归最小二乘(RLS)算法在线识别电机电感值
 **********************************/

#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif

#include <math.h>

// 全局变量
float l_h;      // 采样时间
float l_Pn0;    // 前一时刻的协方差矩阵
float l_Pn1;    // 当前时刻的协方差矩阵
float l_x1;     // 状态变量1（电感估计值）
float l_x2;     // 状态变量2（协方差矩阵）
float l_K;      // 卡尔曼增益
float l_theta0; // 前一时刻的电感估计值
float l_theta1; // 当前时刻的电感估计值
float l_Ud;     // d轴电压输入

// 输入输出维度定义
#define u_width 2  // 输入维度：[采样时间, d轴电压]
#define y_width 1  // 输出维度：[电感估计值]

/**
 * @brief   电感识别初始化函数
 * @param   xD: 状态变量数组
 * @note    初始化电感估计值和协方差矩阵
 */
void L_identification_Start_wrapper(real_T *xD)
{
    // 初始化电感估计值，初始值设为0.01H
    l_theta0 = 0.01f;
    // 初始化协方差矩阵
    l_Pn0 = 0.0008f * 2.0f;

    // 初始化状态变量
    l_x1 = l_theta0;  // 状态1：电感估计值
    l_x2 = l_Pn0;     // 状态2：协方差矩阵
}

/**
 * @brief   电感识别输出函数
 * @param   u: 输入数组 [采样时间, d轴电压]
 * @param   y: 输出数组 [电感估计值]
 * @param   xD: 状态变量数组
 * @note    输出当前的电感估计值
 */
void L_identification_Outputs_wrapper(const real32_T *u,
            real32_T *y,
            const real_T *xD)
{
    // 获取当前的电感估计值
    l_theta1 = l_x1;
    // 输出电感估计值
    y[0] = l_theta1;
}

/**
 * @brief   电感识别更新函数
 * @param   u: 输入数组 [采样时间, d轴电压]
 * @param   y: 输出数组 [电感估计值]
 * @param   xD: 状态变量数组
 * @note    使用递归最小二乘(RLS)算法更新电感估计值
 */
void L_identification_Update_wrapper(const real32_T *u,
            real32_T *y,
            real_T *xD)
{
    // 获取输入参数
    l_h = u[0];    // 采样时间
    l_Ud = u[1];   // d轴电压

    // 获取前一时刻的状态
    l_Pn0 = l_x2;  // 前一时刻的协方差矩阵
    l_theta0 = l_x1; // 前一时刻的电感估计值

    // 计算卡尔曼增益
    l_K = l_Pn0 * l_h / (1 + l_h * l_Pn0 * l_h);

    // 更新协方差矩阵
    l_Pn1 = l_Pn0 - l_K * l_h * l_Pn0;

    // 更新电感估计值
    // 公式：theta1 = theta0 + K * (Ud - h * 0.99 * theta0)
    // 其中0.99是一个调整系数，用于减少噪声对估计的影响
    l_theta1 = l_theta0 + l_K * (l_Ud - l_h * 0.99f * l_theta0);

    // 更新状态变量
    l_x1 = l_theta1;  // 更新后的电感估计值
    l_x2 = l_Pn1;     // 更新后的协方差矩阵
}

/**********************************
 * 电感识别算法原理
 * 
 * 1. 基本原理：
 *    - 使用递归最小二乘(RLS)算法在线识别电机电感值
 *    - 基于电机d轴电压方程：Ud = R*Id + L*dId/dt
 *    - 当Id为恒定值时，dId/dt = 0，方程简化为Ud = R*Id
 *    - 当Id变化时，通过测量Ud和Id的变化来估计L
 *
 * 2. 数学推导：
 *    a. 从d轴电压方程出发
 *       Ud = R*Id + L*dId/dt
 *    b. 移项得到
 *       Ud - R*Id = L*dId/dt
 *    c. 令y = Ud - R*Id，u = dId/dt，则
 *       y = L*u
 *    d. 这是一个线性模型，其中L是待估计的参数
 *    e. 使用递归最小二乘(RLS)算法估计L值
 *       - 初始化：θ0 = 初始电感估计值，P0 = 初始协方差矩阵
 *       - 预测：ŷk = θk-1*uk
 *       - 预测误差：ek = yk - ŷk
 *       - 计算增益：Kk = Pk-1*uk/(1 + uk*Pk-1*uk)
 *       - 更新估计：θk = θk-1 + Kk*ek
 *       - 更新协方差：Pk = Pk-1 - Kk*uk*Pk-1
 *
 * 3. 算法流程：
 *    - 初始化电感估计值和协方差矩阵
 *    - 每周期采样时，获取d轴电压和采样时间，计算卡尔曼增益
 *    - 使用卡尔曼增益更新电感估计值和协方差矩阵
 *    - 输出更新后的电感估计值
 *
 * 4. 优点：
 *    - 实现实时识别，能够适应电机参数的变化
 *    - 算法简单，计算量小，适合嵌入式系统使用
 *    - 不需要专门的测试信号，可以在电机正常运行时进行识别
 **********************************/