/**********************************
 * 电阻和磁链识别模块
 * 功能：使用递归最小二乘(RLS)算法在线估计电机电阻和磁链值
 **********************************/

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

#include <math.h>

// 全局变量
float theta0_1_1; // 前一时刻的电阻估计值
float theta0_2_1; // 前一时刻的磁链估计值
float Pn0_1_1;    // 误差协方差矩阵元素 (1,1)
float Pn0_1_2;    // 误差协方差矩阵元素 (1,2)
float Pn0_2_1;    // 误差协方差矩阵元素 (2,1)
float Pn0_2_2;    // 误差协方差矩阵元素 (2,2)
float x_1_1;      // 状态变量1（电阻估计值）
float x_1_2;      // 状态变量2（协方差矩阵元素 (1,1)）
float x_1_3;      // 状态变量3（协方差矩阵元素 (1,2)）
float x_2_1;      // 状态变量4（磁链估计值）
float x_2_2;      // 状态变量5（协方差矩阵元素 (2,1)）
float x_2_3;      // 状态变量6（协方差矩阵元素 (2,2)）
float h_1_1;      // 输入1（q轴电流）
float h_2_1;      // 输入2（电角速度）
float r_K_1_1;    // 卡尔曼增益1
float r_K_2_1;    // 卡尔曼增益2
float Pn1_1_1;    // 更新后的协方差矩阵元素 (1,1)
float Pn1_1_2;    // 更新后的协方差矩阵元素 (1,2)
float Pn1_2_1;    // 更新后的协方差矩阵元素 (2,1)
float Pn1_2_2;    // 更新后的协方差矩阵元素 (2,2)
float theta1_1_1; // 当前时刻的电阻估计值
float theta1_2_1; // 当前时刻的磁链估计值
float r_temp_1_1; // 临时变量1
float r_temp_1_2; // 临时变量2
float r_temp_2_1; // 临时变量3
float r_temp_2_2; // 临时变量4
float r_Uq;       // q轴电压输入

// 输入输出宽度定义
#define u_width 3  // 输入宽度：[q轴电流, 电角速度, q轴电压]
#define y_width 2  // 输出宽度：[电阻估计值, 磁链估计值]

/**
 * @brief   电阻和磁链识别初始化函数
 * @param   xD: 状态变量数组
 * @note    初始化电阻和磁链估计值以及误差协方差矩阵
 */
void R_flux_identification_Start_wrapper(real_T *xD)
{
    // 初始化电阻估计值（初始值设为0.05Ω）
    theta0_1_1 = 0.05f;
    // 初始化磁链估计值（初始值设为0.01Wb）
    theta0_2_1 = 0.01f;

    // 初始化误差协方差矩阵
    Pn0_1_1 = 0.0008f * 2.0f;  // 协方差矩阵 (1,1)
    Pn0_1_2 = 0.0008f * 0.0f;  // 协方差矩阵 (1,2)
    Pn0_2_1 = 0.0008f * 0.0f;  // 协方差矩阵 (2,1)
    Pn0_2_2 = 0.0008f * 2.0f;  // 协方差矩阵 (2,2)

    // 初始化状态变量
    x_1_1 = theta0_1_1;  // 状态1：电阻估计值
    x_2_1 = theta0_2_1;  // 状态2：磁链估计值
    x_1_2 = Pn0_1_1;     // 状态3：协方差矩阵 (1,1)
    x_1_3 = Pn0_1_2;     // 状态4：协方差矩阵 (1,2)
    x_2_2 = Pn0_2_1;     // 状态5：协方差矩阵 (2,1)
    x_2_3 = Pn0_2_2;     // 状态6：协方差矩阵 (2,2)
}

/**
 * @brief   电阻和磁链识别输出函数
 * @param   u: 输入数组 [q轴电流, 电角速度, q轴电压]
 * @param   y: 输出数组 [电阻估计值, 磁链估计值]
 * @param   xD: 状态变量数组
 * @note    输出当前的电阻和磁链估计值
 */
void R_flux_identification_Outputs_wrapper(const real32_T *u,
            real32_T *y,
            const real_T *xD)
{
    // 输出电阻估计值
    y[0] = x_1_1;
    // 输出磁链估计值
    y[1] = x_2_1;
}

/**
 * @brief   电阻和磁链识别更新函数
 * @param   u: 输入数组 [q轴电流, 电角速度, q轴电压]
 * @param   y: 输出数组 [电阻估计值, 磁链估计值]
 * @param   xD: 状态变量数组
 * @note    使用递归最小二乘(RLS)算法更新电阻和磁链估计值
 */
void R_flux_identification_Update_wrapper(const real32_T *u,
            real32_T *y,
            real_T *xD)
{
    // 获取输入参数
    h_1_1 = u[0];    // q轴电流
    h_2_1 = u[1];    // 电角速度
    r_Uq = u[2];     // q轴电压

    // 获取前一时刻的状态
    Pn0_1_1 = x_1_2;  // 协方差矩阵 (1,1)
    Pn0_1_2 = x_1_3;  // 协方差矩阵 (1,2)
    Pn0_2_1 = x_2_2;  // 协方差矩阵 (2,1)
    Pn0_2_2 = x_2_3;  // 协方差矩阵 (2,2)

    // 计算卡尔曼增益
    r_temp_1_1 = 1.0f + (((h_1_1 * Pn0_1_1 + h_2_1 * Pn0_2_1) * h_1_1) + ((h_1_1 * Pn0_1_2 + h_2_1 * Pn0_2_2) * h_2_1));
    r_temp_1_1 = 1.0f / r_temp_1_1;
    r_K_1_1 = (Pn0_1_1 * h_1_1 + Pn0_1_2 * h_2_1) * r_temp_1_1;
    r_K_2_1 = (Pn0_2_1 * h_1_1 + Pn0_2_2 * h_2_1) * r_temp_1_1;

    // 更新误差协方差矩阵
    r_temp_1_1 = r_K_1_1 * h_1_1;
    r_temp_1_2 = r_K_1_1 * h_2_1;
    r_temp_2_1 = r_K_2_1 * h_1_1;
    r_temp_2_2 = r_K_2_1 * h_2_1;
    Pn1_1_1 = Pn0_1_1 - (r_temp_1_1 * Pn0_1_1 + r_temp_1_2 * Pn0_2_1);
    Pn1_1_2 = Pn0_1_2 - (r_temp_1_1 * Pn0_1_2 + r_temp_1_2 * Pn0_2_2);
    Pn1_2_1 = Pn0_2_1 - (r_temp_2_1 * Pn0_1_1 + r_temp_2_2 * Pn0_2_1);
    Pn1_2_2 = Pn0_2_2 - (r_temp_2_1 * Pn0_1_2 + r_temp_2_2 * Pn0_2_2);

    // 获取前一时刻的估计值
    theta0_1_1 = x_1_1;  // 前一时刻的电阻估计值
    theta0_2_1 = x_2_1;  // 前一时刻的磁链估计值

    // 更新电阻和磁链估计值
    // 公式：theta1 = theta0 + K * (Uq - (h1*theta0_1*0.999 + h2*theta0_2*0.999))
    // 其中0.999是一个近似系数，用于考虑其他因素的影响
    theta1_1_1 = theta0_1_1 + r_K_1_1 * (r_Uq - (h_1_1 * theta0_1_1 * 0.999f + h_2_1 * theta0_2_1 * 0.999f));
    theta1_2_1 = theta0_2_1 + r_K_2_1 * (r_Uq - (h_1_1 * theta0_1_1 * 0.999f + h_2_1 * theta0_2_1 * 0.999f));

    // 更新状态变量
    x_1_1 = theta1_1_1;  // 更新电阻估计值
    x_2_1 = theta1_2_1;  // 更新磁链估计值
    x_1_2 = Pn1_1_1;     // 更新协方差矩阵 (1,1)
    x_1_3 = Pn1_1_2;     // 更新协方差矩阵 (1,2)
    x_2_2 = Pn1_2_1;     // 更新协方差矩阵 (2,1)
    x_2_3 = Pn1_2_2;     // 更新协方差矩阵 (2,2)
}

/**********************************
 * 电阻和磁链识别算法原理
 * 
 * 1. 基本原理：
 *    - 使用递归最小二乘(RLS)算法在线估计电机的电阻和磁链值
 *    - 基于电机的q轴电压方程：Uq = R*Iq + L*dIq/dt + ω*ψ
 *    - 当Iq为恒定值时，dIq/dt = 0，方程简化为Uq = R*Iq + ω*ψ
 *    - 通过测量Uq、Iq和ω来同时估计R和ψ
 *
 * 2. 数学推导：
 *    a. 从q轴电压方程开始（假设dIq/dt ≈ 0）：
 *       Uq = R*Iq + ω*ψ
 *    b. 令y = Uq，φ = [Iq, ω]，θ = [R, ψ]，则：
 *       y = φ^T * θ
 *    c. 这是一个线性模型，其中R和ψ是待估计的参数
 *    d. 使用递归最小二乘(RLS)算法估计θ：
 *       - 初始化：θ0 = [R0, ψ0]^T，P0 = 初始误差协方差矩阵
 *       - 预测：?k = φk^T * θk-1
 *       - 预测误差：ek = yk - ?k
 *       - 卡尔曼增益：Kk = Pk-1*φk/(1 + φk^T*Pk-1*φk)
 *       - 参数更新：θk = θk-1 + Kk*ek
 *       - 协方差更新：Pk = (I - Kk*φk^T)*Pk-1
 *
 * 3. 算法流程：
 *    - 初始化电阻和磁链估计值以及误差协方差矩阵
 *    - 每次采样时，根据输入的q轴电流、电角速度和q轴电压计算卡尔曼增益
 *    - 使用卡尔曼增益更新电阻和磁链估计值以及误差协方差矩阵
 *    - 输出更新后的电阻和磁链估计值
 *
 * 4. 优点：
 *    - 在线实时估计，能够适应电机参数的变化
 *    - 同时估计电阻和磁链两个参数
 *    - 计算量适中，适合在嵌入式系统中使用
 *    - 不需要专门的测试信号，可以在正常运行时进行估计
 **********************************/