【MATLAB例程】扩展卡尔曼滤波估计pmsm的位置，输出估计位置速度的曲线对比、误差对比

关注

原创 2026-04-23 10:51:50 1.2k 阅读 AI 写同款· GEO 优化 ›

【电机滤波代码4】基于Huber权重的鲁棒扩展卡尔曼滤波（EKF）原理与MATLAB例程：PMSM电流离群点降权。订阅专栏后可查看完整代码本文实现基于胡贝尔权重的鲁棒扩展卡尔曼滤波（EKF），用于PMSM电流离群点抑制。通过仿真生成含脉冲异常的电流数据，利用标准化新息计算鲁棒权重，动态调整测量协方差以降低异常观测影响。代码集成数据生成、滤波递推、可视化与误差评估于一体，支持单文件运行，适用于课程实验与算法二次开发。结果展示转速、电角度、负载及电流估计性能，验证了在稀疏异常下滤波器的稳定性与精度。订阅专栏可获取完整MATLAB源码。阅读详情

在这里插入图片描述

基于扩展卡尔曼滤波（EKF）估计永磁同步电机（PMSM）位置误差的 MATLAB 实现和说明。代码分为系统建模、EKF 算法实现和仿真验证三部分。

代码实现基于**扩展卡尔曼滤波（EKF）**的永磁同步电机转子位置与速度的无传感器估计。通过建立PMSM的动力学模型，结合电压输入与电流观测，算法能够实时预测电机的转子位置（θ）、转速（ω）及交直轴电流（id/iq），有效解决了传统传感器依赖带来的成本和可靠性问题。

运行结果

在这里插入图片描述

系统建模

建立 PMSM 的状态方程和输出方程，并进行离散化处理。

状态方程和离散化
- 使用非线性状态方程 $f (x, u)$ 模拟 PMSM 的动态行为，并在 EKF 中离散化处理。
噪声建模
- 过程噪声 $Q$ 和测量噪声 $R$ 是 EKF 的关键参数，需根据实际系统调整。

MATLAB 代码

% PMSM 无传感器位置估计（EKF）
% 优化版：修正雅可比矩阵、变量命名冲突、添加角度归一化与完整可视化
% 2025-05-23 / Ver2

clc; clear; close all;
rng(0);

%% ===== 系统参数 =====
np   = 4;       % 极对数（避免与协方差矩阵 P 命名冲突）
Rs   = 0.2;     % 定子电阻 (Ω)
Ld   = 0.002;   % d 轴电感 (H)
Lq   = 0.002;   % q 轴电感 (H)
lf   = 0.1;     % 永磁磁链 λ_f (Wb)
J    = 0.001;   % 转动惯量 (kg·m²)
B    = 0.0001;  % 粘性摩擦系数 (N·m·s/rad)
Ts   = 1e-4;    % 采样时间 (s)

%% ===== 状态方程与输出方程 =====
f = @(x, u) [
    x(1) + Ts * x(2);
    x(2) + Ts*(3*np/2*(lf*x(4) + (Ld-Lq)*x(3)*x(4))/J - B*x(2)/J);
    x(3) + Ts*(u(1) - Rs*x(3) + x(2)*Lq*x(4))/Ld;
    x(4) + Ts*(u(2) - Rs*x(4) - x(2)*(Ld*x(3) + lf))/Lq
];
h = @(x) [x(3); x(4)];  % 观测：dq 轴电流

%% ===== EKF 噪声参数 =====
Q_cov = diag([1e-6, 1e-6, 1e-4, 1e-4]);  % 过程噪声协方差
R_cov = diag([1e-3, 1e-3]);               % 测量噪声协方差

%% ===== 初始化 =====
x_true = zeros(4,1);   % 真实状态 [θ, ω, id, iq]
x_hat  = zeros(4,1);   % EKF 估计状态
P      = diag([1e-4, 1e-4, 1e-2, 1e-2]);  % 初始状态协方差

%% ===== 仿真设置 =====
T_end = 1;
N     = T_end / Ts;
t     = (0:N-1) * Ts;

vd = 10 * sin(2*pi*50*t);
vq = 10 * cos(2*pi*50*t);

%% ===== 数据记录 =====
log_theta_true = zeros(1,N);
log_theta_hat  = zeros(1,N);
log_omega_true = zeros(1,N);
log_omega_hat  = zeros(1,N);
log_id_true    = zeros(1,N);
log_id_hat     = zeros(1,N);
log_theta_err  = zeros(1,N);

%% ===== EKF 主循环 =====
for k = 1:N
    u = [vd(k); vq(k)];

    % --- 真实系统更新（含过程噪声）---
    x_true = f(x_true, u) + chol(Q_cov)' * randn(4,1);
    x_true(1) = mod(x_true(1), 2*pi);   % 角度归一化

    % --- 带噪声测量 ---
    z = h(x_true) + chol(R_cov)' * randn(2,1);

    % ===== EKF 预测步骤 =====
    x_pred = f(x_hat, u);

    % 解析计算状态相关雅可比矩阵 F = ∂f/∂x|x̂
    w  = x_hat(2);   % 当前估计转速
    id = x_hat(3);   % 当前估计 id
    iq = x_hat(4);   % 当前估计 iq