当前位置：首页>学习笔记>学习笔记1-无感FOC -拓展卡尔曼滤波EKF解析

学习笔记1-无感FOC -拓展卡尔曼滤波EKF解析

2026-03-30 20:50:20

最近学习了Foc的高阶无感算法，拓展卡尔曼滤波算法，下面对其核心内容进行解析说明：

EKF的核心：通过协方差矩阵，去计算估算值和实测值之间的方差，通过协方差矩阵计算卡尔曼增益看，通过卡尔曼增益K，对测量值和估算值进行重新分配得到最优估算，估算值要优于实测值和估算值。

EKF的5大公式：

EKF解析-公式1：

核心说明：状态估算实际和EKF的逻辑没有任何关系，估算是通过α、β轴的电机方程+离散化的方式得到。

αβ 轴连续时域核心方程

前向欧拉离散化

公式一实际估算方程
x_pred[0]= iα + Ts*(-Rs/Ls*iα + ωe*PSIf/Ls*sinf(θe)+ uα/Ls); x_pred[1]= iβ + Ts*(-Rs/Ls*iβ - ωe*PSIf/Ls*cosf(θe)+ uβ/Ls); x_pred[2]= ωe;
x_pred[3]= θe + Ts*ωe;

EKF解析-公式2：

核心说明：通过公式1得到的预测值（转子电角度和速度），计算雅可比矩阵，通过雅可比运算（线性化）+Q矩阵（模型误差），实现协方差P的更新。为第三步的卡尔曼增益k做准备

协方差计算

雅可比矩阵

Q矩阵（模型误差）
主要来源于死区，模型观测问题，信号链路延迟，工程常用Q矩阵
EKF解析-公式3：
核心说明：通过第二步更新的协方差，去映射模型误差在采样角度的误差，之后通过K=模型误差（电流采样角度）/模型误差+R（采样误差）

R测量误差
电流采样的误差，采集进去是ia ib，通过基尔霍夫KCL定理和Clark运算，得到iα和iβ。实际由于采样精度问题，会存在误差
H矩阵（估算的iα和iβ）
本质上是第一步估算的iα和 iβ：
x_pred[0]= iα + Ts*(-Rs/Ls*iα + ωe*PSIf/Ls*sinf(θe)+ uα/Ls); x_pred[1]= iβ + Ts*(-Rs/Ls*iβ - ωe*PSIf/Ls*cosf(θe)+ uβ/Ls);

HPHT

通过协方差来映射在测量角度看的，估算误差带来的实际策略误差（本质上就为了和R测量误差矩阵保持同一个平台角度）

计算卡尔曼增益K值

EKF解析-公式4：
核心说明：通过卡尔曼增益k，对模型预测值和传感器实测值重新分配，得到更精准的数据。
这里存在一个核心问题，对iα和iβ是可以通过传感器得到，为什么可以计算更重要的电角度θ和转速w？
因为模型建议电流、电角度、转速，是完全锁死的。也就是电流模型准确，导致转速和电角度也是准确的，具体为什么，还因为电机模型：

公式，第四步是最简单，本质是通过k值去调节观测值
新的观测值（本轮最终输出值）=旧的观测器值+k*（αβ电流实测值-αβ电流观测值）

EKF解析-公式5：
核心说明：通过卡尔曼增益k，对协方差进行更新，为下一轮的EKF运算做准备。本质目的是为了最终的协方差收敛趋近于0

EKF最终的目的是，为了使得协方差p矩阵，接近于0矩阵。因为协方差矩阵的主对角线表示的就是iα电流、iβ电流、电角度θ、转速w

下面是关键代码解析：

// 协方差矩阵 Pfloat P[4][4] = {    {10,0,0,0},    {0,10,0,0},    {0,0,100,0},    {0,0,0,100}};// 过程噪声 Qfloat Q[4][4] = {    {0.1f,0,0,0},    {0,0.1f,0,0},    {0,0,10.0f,0},    {0,0,0,0.5f}};// 观测噪声 R（电流采样噪声）float R[2][2] = {    {0.5f,0},    {0,0.5f}};// 单位矩阵float I[4][4] = {    {1,0,0,0},    {0,1,0,0},    {0,0,1,0},    {0,0,0,1}};//========================= 矩阵工具函数（简化版） =========================//void Matrix_Mul4x4(float A[4][4], float B[4][4], float out[4][4]);void Matrix_Mul4x4T(float A[4][4], float B[4][4], float out[4][4]); // A*B^Tvoid Matrix_Add4x4(float A[4][4], float B[4][4], float out[4][4]);float Matrix_Det2x2(float m[2][2]);void Matrix_Inv2x2(float m[2][2], float out[2][2]);//========================= EKF 主函数（5大步骤完整实现） =========================//// 输入：uα, uβ 电机电压// 输入：iα_sam, iβ_sam 电流采样void EKF_Step(float uα, float uβ, float iα_sam, float iβ_sam){    // 保存上一时刻状态    float iα_prev = x[0];    float iβ_prev = x[1];    float ωe_prev = x[2];    float θe_prev = x[3];    //=====================================================================    // 【第一步：状态预测（先验估计）】    // 公式：x_pred = f(x_prev, u)    // 完全使用电机 αβ 方程计算    //=====================================================================    float x_pred[4];    x_pred[0] = iα_prev + Ts*(-Rs/Ls*iα_prev + ωe_prev*PSIf/Ls*sinf(θe_prev) + uα/Ls);    x_pred[1] = iβ_prev + Ts*(-Rs/Ls*iβ_prev - ωe_prev*PSIf/Ls*cosf(θe_prev) + uβ/Ls);    x_pred[2] = ωe_prev;                        // 短时间转速不变    x_pred[3] = θe_prev + Ts*ωe_prev;            // 角度 = 角度 + 转速*时间    // 角度限幅 -π ~ π    if(x_pred[3] >  M_PI) x_pred[3] -= 2*M_PI;    if(x_pred[3] < -M_PI) x_pred[3] += 2*M_PI;    //=====================================================================    // 【第二步：计算雅可比矩阵 F】    // 作用：非线性模型线性化，描述状态间误差耦合    //=====================================================================    float F[4][4] = {0};    float F11 = 1 - Rs*Ts/Ls;    float F13 = PSIf*Ts/Ls*sinf(θe_prev);    float F14 = PSIf*Ts/Ls*ωe_prev*cosf(θe_prev);    float F22 = 1 - Rs*Ts/Ls;    float F23 = -PSIf*Ts/Ls*cosf(θe_prev);    float F24 = PSIf*Ts/Ls*ωe_prev*sinf(θe_prev);    F[0][0] = F11;   F[0][1] = 0;    F[0][2] = F13;   F[0][3] = F14;    F[1][0] = 0;     F[1][1] = F22;  F[1][2] = F23;   F[1][3] = F24;    F[2][0] = 0;     F[2][1] = 0;    F[2][2] = 1;     F[2][3] = 0;    F[3][0] = 0;     F[3][1] = 0;    F[3][2] = Ts;    F[3][3] = 1;    //=====================================================================    // 【第三步：协方差预测】    // 公式：P_pred = F*P*F.T + Q    // 作用：把上一轮误差按电机模型传播放大    //=====================================================================    float P_temp1[4][4], P_temp2[4][4], P_pred[4][4];    Matrix_Mul4x4(F, P, P_temp1);         // F * P    Matrix_Mul4x4T(P_temp1, F, P_temp2);  // F*P * F^T    Matrix_Add4x4(P_temp2, Q, P_pred);    // + Q    //=====================================================================    // 【第四步：计算卡尔曼增益 K】    // 公式：K = P_pred*H.T / (H*P_pred*H.T + R)    // 作用：自动决定信任模型还是信任传感器    // H = [[1,0,0,0],[0,1,0,0]]    //=====================================================================    float S[2][2], S_inv[2][2];    S[0][0] = P_pred[0][0] + R[0][0];    S[0][1] = P_pred[0][1] + R[0][1];    S[1][0] = P_pred[1][0] + R[1][0];    S[1][1] = P_pred[1][1] + R[1][1];    Matrix_Inv2x2(S, S_inv);  // 求逆    // K = P_pred * H.T * S_inv    float K[4][2];    K[0][0] = P_pred[0][0] * S_inv[0][0] + P_pred[0][1] * S_inv[1][0];    K[0][1] = P_pred[0][0] * S_inv[0][1] + P_pred[0][1] * S_inv[1][1];    K[1][0] = P_pred[1][0] * S_inv[0][0] + P_pred[1][1] * S_inv[1][0];    K[1][1] = P_pred[1][0] * S_inv[0][1] + P_pred[1][1] * S_inv[1][1];    K[2][0] = P_pred[2][0] * S_inv[0][0] + P_pred[2][1] * S_inv[1][0];    K[2][1] = P_pred[2][0] * S_inv[0][1] + P_pred[2][1] * S_inv[1][1];    K[3][0] = P_pred[3][0] * S_inv[0][0] + P_pred[3][1] * S_inv[1][0];    K[3][1] = P_pred[3][0] * S_inv[0][1] + P_pred[3][1] * S_inv[1][1];    //=====================================================================    // 【第五步：状态更新】    // 公式：x_new = x_pred + K*(z - H*x_pred)    // 作用：用电流误差修正 iα、iβ、ωe、θe    //=====================================================================    float res[2];    res[0] = iα_sam - x_pred[0];   // iα 残差    res[1] = iβ_sam - x_pred[1];   // iβ 残差    x[0] = x_pred[0] + K[0][0]*res[0] + K[0][1]*res[1];    x[1] = x_pred[1] + K[1][0]*res[0] + K[1][1]*res[1];    x[2] = x_pred[2] + K[2][0]*res[0] + K[2][1]*res[1];  // 修正转速    x[3] = x_pred[3] + K[3][0]*res[0] + K[3][1]*res[1];  // 修正角度    // 角度限幅    if(x[3] >  M_PI) x[3] -= 2*M_PI;    if(x[3] < -M_PI) x[3] += 2*M_PI;    //=====================================================================    // 【第六步：协方差更新】    // 公式：P_new = (I - K*H)*P_pred    // 作用：压缩误差，给下一轮使用    //=====================================================================    float KH[4][4] = {0};    KH[0][0] = K[0][0]; KH[0][1] = K[0][1];    KH[1][0] = K[1][0]; KH[1][1] = K[1][1];    KH[2][0] = K[2][0]; KH[2][1] = K[2][1];    KH[3][0] = K[3][0]; KH[3][1] = K[3][1];    // I - K*H    float I_KH[4][4];    for(int i=0; i<4; i++)        for(int j=0; j<4; j++)            I_KH[i][j] = I[i][j] - KH[i][j];    // P = (I-KH)*P_pred    Matrix_Mul4x4(I_KH, P_pred, P);}