function PMSM_Motor_EKF_Kalman % Discrete-time extended Kalman filter simulation for two-phase % step motor. Estimate the stator currents, and the rotor position % and velocity, on the basis of noisy measurements of the stator % currents. % % Sistemi Dinamici - a.a. 2022-2023 % ================================================================ % Credits: % Dan Simon, Optimal State Estimation, Wiley & Sons, 2006 % % ---------- PMSM Drive Parameters -------------- Ra = 1.9; % Winding resistance L = 0.003; % Winding inductance lambda = 0.1; % Motor constant J = 0.00018; % Moment of inertia B = 0.001; % Coefficient of viscous friction % ----------------------------------------------- % ----- Time Intervals ---- dt = 1e-5; % ODE Simulation step size (seconds) T = 1e-3; % how often measurements are obtained <- EKF innovation % acquisition % Experiments: try to use % dt = 1e-5 AND T = 5e-3 <--> What happens? % dt = 1e-3 AND T = 1e-3 <--> What happens? % Try with different values for dt and T. What happens? tf = 4; % Simulation length % -------------------------- % ---- Process and Measurements Noise Covariance Matrices ------------- ControlNoise = 1e-3; % std dev of uncertainty in control inputs (amps) AccelNoise = 5e-2; % std dev of shaft acceleration noise (rad/sec^2) MeasNoise = 1e-1; % standard deviation of % measurement noise (amps) % Try with different values for the process and/or measurement noise % standard deviations. What happens? % ------- Measurement Noise Covariance ----- V2 = [ MeasNoise^2 0;... 0 MeasNoise^2]; % ------------------------------------------- % ------ Process Noise Covariance Matrix ----------------------- xdotNoise = [ControlNoise/L; ControlNoise/L; AccelNoise; 0]; % see Eq. L13-p95 V1 = [xdotNoise(1)^2 0 0 0; 0 xdotNoise(2)^2 0 0; 0 0 xdotNoise(3)^2 0; 0 0 0 xdotNoise(4)^2]*T; % -------------------------------------------------------------- % ---- Initial state, State Estimate and Covariance -------- P = 1*eye(4); % Initial state estimation covariance x = zeros(4,1); % Initial state xhat = x; % Initial state estimate % ---------------------------------------------------------- w = 2 * pi; % Control input frequency dtPlot = 1e-2; % How often to plot results tPlot = -inf; % Most recent time at which data was saved for plotting % Initialize arrays for plotting at the end of the program xArray = []; yArray = []; xhatArray = []; trPArray = []; tArray = []; % ------- ODE Simulation Loop --------------------------------------- for t = 0 : T : tf-T+eps % Noisy measurement acquisition y = [x(1); x(2)] + MeasNoise * randn(2,1); % Save data for plotting if (t >= (tPlot + dtPlot)) tPlot = t + dtPlot - eps; % update tPlot % store the values in the proper arrays xArray = [xArray x]; yArray = [yArray y]; xhatArray = [xhatArray xhat]; trPArray = [trPArray trace(P)]; tArray = [tArray t]; end % ------------------------------------------------------------------- % System simulation --> solving the ODE with integration time step dt % ODE time interval: [t, t+T-dt[ for tau = 0 : dt : T-dt+eps time = t + tau; % update the time for the ODE ua = sin(w*time); % the actual value of the input voltages ub = cos(w*time); % without the noise contributions <-> see Eq. L13-p95 % let's update the state x(T+1) <-- (Eq. L13-p95) xdot = [-Ra/L*x(1) + x(3)*lambda/L*sin(x(4)) + ua/L; -Ra/L*x(2) - x(3)*lambda/L*cos(x(4)) + ub/L; -3/2*lambda/J*x(1)*sin(x(4)) + ... 3/2*lambda/J*x(2)*cos(x(4)) - B/J*x(3); x(3)]; %xdot = xdot + xdotNoise .* randn(4,1); x = x + xdot * dt; % rectangular integration x <-> x(T+1) x = x + [xdotNoise(1)*randn; ... xdotNoise(2)*randn; ... xdotNoise(3)*randn; ... xdotNoise(4)*randn] * sqrt(dt); % adding process noise x(4) = mod(x(4), 2*pi); % keep the angle between 0 and 2*pi end % ------------------------------------------------------------------- ua = sin(w*t); ub = cos(w*t); % Compute the partial derivative matrices -- continuous time -- A = [-Ra/L 0 lambda/L*sin(xhat(4)) xhat(3)*lambda/L*cos(xhat(4)); 0 -Ra/L -lambda/L*cos(xhat(4)) xhat(3)*lambda/L*sin(xhat(4)); -3/2*lambda/J*sin(xhat(4)) 3/2*lambda/J*cos(xhat(4)) -B/J ... -3/2*lambda/J*(xhat(1)*cos(xhat(4))+xhat(2)*sin(xhat(4))); 0 0 1 0]; C = [1 0 0 0; 0 1 0 0]; % Compute the discrete-time matrices F = eye(size(A))+T*A; % <-- L13-p95 H = C; % Compute the Kalman filter gain HT = H'; K0 = P * HT * inv(H * P * HT + V2); % <-- L13-p61 % Update the state estimate deltax = [-Ra/L*xhat(1) + xhat(3)*lambda/L*sin(xhat(4)) + ua/L; -Ra/L*xhat(2) - xhat(3)*lambda/L*cos(xhat(4)) + ub/L; -3/2*lambda/J*xhat(1)*sin(xhat(4)) + ... 3/2*lambda/J*xhat(2)*cos(xhat(4)) - B/J*xhat(3); xhat(3)] * T; xhat = xhat + deltax + K0 * (y - [xhat(1); xhat(2)]); % keep the angle estimate between 0 and 2*pi xhat(4) = mod(xhat(4), 2*pi); % Update the estimation error covariance. % Don't delete the extra parentheses (numerical issues). P = F*(P-P*HT*inv(V2+H*P*HT)*H*P)*F'+V1; % <-- L13-p82 end % Plot data. close all; figure("Name", 'PMSM drive state', ... "Units","normalized","Position",[0.1, 0.1, 0.8, 0.8]); set(gcf,'Color','White'); hg(1) = subplot(2,2,1); hold on; box on; plot(tArray, yArray(1,:), 'b-', 'LineWidth', 2); plot(tArray, xArray(1,:), 'g-', 'LineWidth', 2); plot(tArray, xhatArray(1,:), 'r:', 'LineWidth', 2) set(gca,'FontSize',18); ylabel('Current A (Amps)'); legend('Measured', 'True','Estimated'); hg(2) = subplot(2,2,2); hold on; box on; grid on; zoom on; plot(tArray, yArray(2,:), 'b-', 'LineWidth', 2); plot(tArray, xArray(2,:), 'g-', 'LineWidth', 2); plot(tArray, xhatArray(2,:), 'r:', 'LineWidth', 2) set(gca,'FontSize',18); ylabel('Current B (Amps)'); legend('Measured', 'True','Estimated'); hg(3) = subplot(2,2,3); hold on; box on; grid on; zoom on; plot(tArray, xArray(3,:), 'b-', 'LineWidth', 2); plot(tArray, xhatArray(3,:), 'r:', 'LineWidth', 2) set(gca,'FontSize',18); xlabel('Time (Seconds)'); ylabel('Speed (Rad/Sec)'); legend('True', 'Estimated'); hg(4) = subplot(2,2,4); hold on; box on; grid on; zoom on; plot(tArray, xArray(4,:), 'b-', 'LineWidth', 2); plot(tArray,xhatArray(4,:), 'r:', 'LineWidth', 2) set(gca,'FontSize',18); xlabel('Time (Seconds)'); ylabel('Position (Rad)'); legend('True', 'Estimated'); linkaxes(hg, 'x'); figure("Name",'Trace of the Covariance Matrix P'); plot(tArray, trPArray, 'linewidth', 1.5); grid on; zoom on; title('Trace(P)', 'FontSize', 12); set(gca,'FontSize',18); set(gcf,'Color','White'); xlabel('Seconds'); % Compute the std dev of the estimation errors N = size(xArray, 2); N2 = round(N / 2); xArray = xArray(:,N2:N); xhatArray = xhatArray(:,N2:N); iaEstErr = sqrt(norm(xArray(1,:)-xhatArray(1,:))^2 / size(xArray,2)); ibEstErr = sqrt(norm(xArray(2,:)-xhatArray(2,:))^2 / size(xArray,2)); wEstErr = sqrt(norm(xArray(3,:)-xhatArray(3,:))^2 / size(xArray,2)); thetaEstErr = sqrt(norm(xArray(4,:)-xhatArray(4,:))^2 / size(xArray,2)); disp(['Std Dev of Estimation Errors = ',num2str(iaEstErr),', ',num2str(ibEstErr),', ',num2str(wEstErr),', ',num2str(thetaEstErr)]); % Display the P version of the estimation error standard deviations disp(['Sqrt(P) = ',num2str(sqrt(P(1,1))),', ',num2str(sqrt(P(2,2))),', ',num2str(sqrt(P(3,3))),', ',num2str(sqrt(P(4,4)))]);