【MATLAB代码】扩展卡尔曼滤波估计pmsm的位置误差

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

文章目录

系统建模
- 核心算法解析
- - [1. 系统建模](#1. 系统建模)
  - [2. 噪声配置](#2. 噪声配置)
  - [3. EKF实现流程](#3. EKF实现流程)
  - [4. 输入信号与仿真](#4. 输入信号与仿真)
[MATLAB 代码](#MATLAB 代码)
运行结果

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

系统建模

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

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

核心算法解析

1. 系统建模

状态变量 ：包含转子位置（θ）、转速（ω）、d轴电流（id）、q轴电流（iq），构成4维状态向量x = [θ, ω, id, iq]^T。

状态方程 ：基于PMSM电磁与机械动力学方程，离散化后描述状态演化：

matlab 复制代码

f = @(x, u) [ 
    x(1) + Ts * x(2);  % 位置更新
    x(2) + Ts*(3*P/2*(λ*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;  % d轴电流
    x(4) + Ts*(u(2)-Rs*x(4)-x(2)*(Ld*x(3)+λ))/Lq  % q轴电流
];

该模型考虑了电磁转矩、摩擦阻力及电感特性对状态的影响。

2. 噪声配置

过程噪声协方差Q：对角矩阵，反映各状态变量的过程噪声强度（位置/速度噪声较小，电流噪声较大）。
观测噪声协方差R ：针对电流测量噪声，体现传感器精度限制。
噪声参数的合理设置是保证EKF鲁棒性的关键。

3. EKF实现流程

预测步骤 ：
- 状态预测：通过非线性状态方程更新状态估计。
- 协方差预测：基于简化雅可比矩阵F（此处为恒定矩阵，实际应用中需动态计算状态依赖的雅可比矩阵）：
  matlab 复制代码
```
F = [1, Ts, 0, 0; 0, 1, 0, 0; 0, 0, 1, Ts; 0, 0, 0, 1];
```
更新步骤 ：
- 计算卡尔曼增益K，融合电流观测值（id/iq）修正状态估计。
- 观测雅可比矩阵H为常量，因输出方程仅直接观测电流。

4. 输入信号与仿真

电压输入：采用50Hz正弦（vd）和余弦（vq）信号模拟三相逆变器输出，驱动电机运行。
仿真时长：1秒，采样时间100μs，共10000步，满足实时性需求。

MATLAB 代码

matlab 复制代码

% PMSM估计
% 2025-05-23/Ver1

clc;clear;close all;
rng(0);
% 系统参数设置
P = 4;             % 极对数
Rs = 0.2;          % 定子电阻 (ohm)
Ld = 0.002;        % 直轴电感 (H)
Lq = 0.002;        % 交轴电感 (H)
lambda_f = 0.1;    % 永磁磁链 (Wb)
J = 0.001;         % 转动惯量 (kg·m^2)
B = 0.0001;        % 摩擦系数 (N·m·s/rad)
Ts = 1e-4;         % 采样时间 (s)

% 初始状态 (假设真实值)
theta_true = 0;    % 转子位置 (rad)
omega_true = 0;    % 转子速度 (rad/s)
id_true = 0;       % d轴电流 (A)
iq_true = 0;       % q轴电流 (A)

% 初始估计值
theta_hat = 0;     % 初始位置估计
omega_hat = 0;     % 初始速度估计
id_hat = 0;        % 初始 d 轴电流估计
iq_hat = 0;        % 初始 q 轴电流估计

% 噪声设置
Q = diag([1e-6, 1e-6, 1e-4, 1e-4]); % 过程噪声协方差
R = diag([1e-3, 1e-3]);             % 测量噪声协方差

% 状态方程
f = @(x, u) [ ...
    x(1) + Ts * x(2); % theta = theta + omega * Ts
    x(2) + Ts * (3*P/2*(lambda_f*x(4) + (Ld-Lq)*x(3)*x(4))/J - B*x(2)/J); % omega
    x(3) + Ts * (u(1) - Rs*x(3) + x(2)*Lq*x(4))/Ld; % id
    x(4) + Ts * (u(2) - Rs*x(4) - x(2)*(Ld*x(3) + lambda_f))/Lq % iq
];

% 输出方程
h = @(x) [x(3); x(4)]; % 输出电流 (id, iq)

% 初始化协方差矩阵
P = diag([1e-4, 1e-4, 1e-2, 1e-2]); % 初始化状态协方差

% 仿真时间
T_end = 1; % 仿真 1 秒
N = T_end / Ts; % 仿真步数

% 输入电压 (正弦波信号)
vd = 10 * sin(2*pi*50*(0:Ts:(T_end-Ts)));
vq = 10 * cos(2*pi*50*(0:Ts:(T_end-Ts)));

% 数据存储
theta_error = zeros(1, N); % 位置误差存储
theta_est = zeros(1, N);   % 估计位置存储
theta_real = zeros(1, N);  % 实际位置存储

% EKF 循环
for k = 1:N
    % 获取输入电压
    u = [vd(k); vq(k)];
    
    % 真实系统更新 (加噪声)
    x_true = [theta_true; omega_true; id_true; iq_true];
    x_true = f(x_true, u) + sqrt(Q) * randn(4, 1); % 真实状态加过程噪声
    theta_true = x_true(1);
    omega_true = x_true(2);
    id_true = x_true(3);
    iq_true = x_true(4);
    
    % 测量值 (加噪声)
    z = h(x_true) + sqrt(R) * randn(2, 1);
    
    % EKF 预测步骤
    x_hat = [theta_hat; omega_hat; id_hat; iq_hat];
    x_pred = f(x_hat, u); % 状态预测
    F = [1, Ts, 0, 0; ... % 状态方程的雅可比矩阵
         0, 1, 0, 0; ...
         0, 0, 1, Ts; ...
         0, 0, 0, 1];
    P_pred = F * P * F' + Q; % 协方差预测
    
    % EKF 更新步骤
    H = [0, 0, 1, 0; ... % 输出方程的雅可比矩阵
         0, 0, 0, 1];
    K = P_pred * H' / (H * P_pred * H' + R); % 卡尔曼增益
    x_hat = x_pred + K * (z - h(x_pred)); % 状态更新
    P = (eye(4) - K * H) * P_pred; % 协方差更新
    
    % 更新估计值
    theta_hat = x_hat(1);
    omega_hat = x_hat(2);
    id_hat = x_hat(3);
    iq_hat = x_hat(4);
    
    % 记录误差
    theta_error(k) = theta_true - theta_hat;
    theta_est(k) = theta_hat;
    theta_real(k) = theta_true;
end

% 绘制真实位置与估计位置
t = 0:Ts:(T_end-Ts);
figure;
subplot(2, 1, 1);
plot(t, theta_real, 'b', 'LineWidth', 1.5); hold on;
plot(t, theta_est, 'r--', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('Position (rad)');
legend('真实位置', '估计位置');
title('PMSM EKF位置估计');
% grid on;

% 绘制位置误差
subplot(2, 1, 2);
plot(t, theta_error, 'k', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('位置误差(rad)');
title('位置估计误差');
% grid on;

运行结果

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者