function?MotorKalman

%?Continuous?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.

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

ControlNoise?=?0.01;?%?std?dev?of?uncertainty?in?control?inputs
AccelNoise?=?0.5;?%?std?dev?of?shaft?acceleration?noise

MeasNoise?=?0.1;?%?standard?deviation?of?measurement?noise
R?=?[MeasNoise^2?0;?0?MeasNoise^2];?%?Measurement?noise?covariance
xdotNoise?=?[ControlNoise/L?ControlNoise/L?0.5?0];
Q?=?[xdotNoise（1）^2?0?0?0;?0?xdotNoise（2）^2?0?0;?0?0?xdotNoise（3）^2?0;?0?0?0?xdotNoise（4）^2];?%?Process?noise?covariance
P?=?1*eye（4）;?%?Initial?state?estimation?covariance

dt?=?0.0005;?%?Integration?step?size
tf?=?1.5;?%?Simulation?length

x?=?[0;?0;?0;?0];?%?Initial?state
xhat?=?x;?%?State?estimate
w?=?2?*?pi;?%?Control?input?frequency

dtPlot?=?0.01;?%?How?often?to?plot?results
tPlot?=?-inf;

%?Initialize?arrays?for?plotting?at?the?end?of?the?program
xArray?=?[];
xhatArray?=?[];
trPArray?=?[];
tArray?=?[];

%?Begin?simulation?loop
for?t?=?0?:?dt?:?tf
????if?t?>=?tPlot?+?dtPlot
????????%?Save?data?for?plotting
????????tPlot?=?t?+?dtPlot?-?eps;
????????xArray?=?[xArray?x];
????????xhatArray?=?[xhatArray?xhat];
????????trPArray?=?[trPArray?trace（P）];
????????tArray?=?[tArray?t];
????end
????%?Nonlinear?simulation
????ua0?=?sin（w*t）;
????ub0?=?cos（w*t）;
????xdot?=?[-Ra/L*x（1）?+?x（3）*lambda/L*sin（x（4））?+?ua0/L;
????????-Ra/L*x（2）?-?x（3）*lambda/L*cos（x（4））?+?ub0/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（1）*randn;?xdotNoise（2）*randn;?xdotNoise（3）*randn;?xdotNoise（4）*randn];
????x?=?x?+?xdot?*?dt;
????x（4）?=?mod（x（4）?2*pi）;
????%?Kalman?filter
????F?=?[-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];
????H?=?[1?0?0?0;?0?1?0?0];
????z?=?H?*?x?+?[MeasNoise*randn;?MeasNoise*randn];
????xhatdot?=?[-Ra/L*xhat（1）?+?xhat（3）*lambda/L*sin（xhat（4））?+?ua0/L;
????????-Ra/L*xhat（2）?-?xhat（3）*lambda/L*cos（xhat（4））?+?ub0/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）];
????xhat?=?xhat?+?xhatdot?*?dt;
????Pdot?=?F?*?P?+?P?*?F‘?+?Q?-?P?*?H‘?*?inv（R）?*?H?*?P;
????P?=?P?+?Pdot?*?dt;
????K?=?P?*?H‘?*?inv（H?*?P?*?H‘?+?R）;
????xhat?=?xhat?+?K?*?（z?-?H?*?xhat）;
????xhat（4）?=?mod（xhat（4）?2*pi）;
end

%?Plot?data.
close?all;
figure;
plot（tArray?xArray（1:）?tArrayxhatArray（1:）‘r:‘）
set（gca‘FontSize‘12）;?set（gcf‘Color‘‘White‘）;
xla

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2017-04-09?12:06??大論文仿真\
?????文件??????187015??2016-07-04?22:05??大論文仿真\Program_SMO2.mdl
?????文件???????66441??2016-06-21?19:28??大論文仿真\SMOgaijintu.mdl
?????文件??????172125??2017-04-05?17:29??大論文仿真\pmsm.mdl
?????目錄???????????0??2017-05-24?21:14??大論文仿真\大論文已經調試好的\
?????文件????????4633??2016-05-29?20:02??大論文仿真\大論文已經調試好的\MotorKalman.m
?????文件??????169615??2017-03-20?21:29??大論文仿真\大論文已經調試好的\Program_SMO.mdl
?????文件??????183287??2017-05-24?13:39??大論文仿真\大論文已經調試好的\Program_SMO1.mdl
?????文件??????186962??2016-08-24?15:29??大論文仿真\大論文已經調試好的\Program_SMO2.mdl
?????文件??????170634??2017-03-01?22:17??大論文仿真\大論文已經調試好的\Program_SMOs.mdl
?????目錄???????????0??2017-04-09?12:06??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\
?????文件?????????222??2010-12-30?18:21??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\CLARKE_DSP.m
?????文件?????????777??2011-01-10?17:10??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\Estimated_Speed.m
?????文件?????????258??2010-12-30?19:55??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\ICLARKE_DSP.m
?????文件?????????232??2010-12-30?18:58??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\IPARK_DSP.m
?????文件?????????247??2010-12-30?18:54??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\PARK_DSP.m
?????文件????????1909??2010-12-24?09:49??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\pid_reg3.m
?????文件??????551636??2016-06-08?11:31??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\plot_MSIP.txt
?????文件?????????724??2011-01-05?13:50??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\ramp_control.m
?????文件???????17367??2011-05-09?14:51??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\sensorless_control_texas_dsp2812_sliding_mode.asv
?????文件???????17365??2011-05-09?14:52??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\sensorless_control_texas_dsp2812_sliding_mode.m
?????文件????????1448??2011-04-14?17:37??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\smopos.m
?????文件?????????900??2011-05-03?11:36??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\speed_Estimated.m
?????文件?????????927??2011-05-03?11:39??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\speed_resolver.m
?????文件????????2980??2011-04-13?17:34??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\svgen_dq.m
?????文件????????1195??2011-04-13?18:15??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\volt_calc.m
?????文件????????1205??2011-04-14?16:55??大論文仿真\大論文已經調試好的\Sensorless_Sliding_mode_English\volt_calc_est.m
?????文件???????19781??2017-03-20?18:21??大論文仿真\大論文已經調試好的\hs_err_pid4664.log
?????文件?????3959747??2017-04-03?14:03??大論文仿真\大論文已經調試好的\plot_MSIP.txt
?????文件??????135472??2017-01-10?11:27??大論文仿真\大論文已經調試好的\zuidadailiubi.mdl
?????目錄???????????0??2017-04-09?12:06??大論文仿真\大論文已經調試好的\鎖相環\
............此處省略16個文件信息