function?Colored（flag?corr）

%?Kalman?filter?simulation?with?colored?（time?correlated）?measurement?noise.
%?INPUTS:
%????flag?=?0?means?ignore?the?time?correlation?of?the?measurement?noise
%???????????1?means?augment?the?system?state
%???????????2?means?use?the?Bryson/Henrikson?approach
%????corr?=?magnitude?of?measurement?noise?correlation?<=?1

kf?=?500;?%?Number?of?time?steps?in?simulation
phi?=?[0.7?-0.15;?0.03?0.79];
H?=?[1?0;?0?1];
psi?=?[corr?0;?0?corr];
L?=?[0.15;?0.21];
Q?=?L?*?[1]?*?L‘;
Qv?=?[0.05?0;?0?0.05];

phi1?=?[phi?zeros（22）;?zeros（22）?psi];
Q1?=?[Q?zeros（22）;?zeros（22）?Qv];
R1?=?zeros（22）;
H1?=?[H?eye（22）];

if?（flag?==?0）
???n?=?2;?%?number?of?states
???R?=?Qv;
???stitle?=?‘Correlation?Ignored‘;
elseif?（flag?==?1）
???n?=?4;?%?number?of?states
???stitle?=?‘Augmented?State‘;
elseif?（flag?==?2）
???n?=?2;?%?number?of?states
???R?=?H?*?Q?*?H‘?+?Qv;
???M?=?Q?*?H‘;
???D?=?H?*?phi?-?psi?*?H;
???stitle?=?‘Bryson?and?Henrikson‘;
else
???disp（‘illegal?input?argument‘）;
end

%?Assume?xhat（0）?=?x（0）?=?0.
x?=?zeros（41）;?xhatplus?=?zeros（n1）;?xhatminus?=?zeros（n1）;
z?=?zeros（21）;
%?Assume?P（0）?=?0.
Pplus?=?zeros（nn）;?Pminus?=?zeros（nn）;

xArray?=?[];?xhatArray?=?[];?KArray?=?[];?PArray?=?[];?zArray?=?[];

randn（‘state‘?0）;
for?k?=?1?:?kf
???%?Simulate?the?system
???StateNoise?=?randn;
???n（11）?=?L（11）?*?StateNoise;
???n（21）?=?L（21）?*?StateNoise;
???n（31）?=?sqrt（Qv（11））?*?randn;
???n（41）?=?sqrt（Qv（22））?*?randn;
???x?=?phi1?*?x?+?n;
???zold?=?z;
???z?=?H1?*?x;
???%?Run?the?Kalman?filter
???if?（flag?==?0）
???????%?Ignore?the?time?correlation
??????Pminus?=?phi?*?Pplus?*?phi‘?+?Q;
??????K?=?inv（H?*?Pminus?*?H‘?+?R）;
??????K?=?（Pminus?*?H‘）?*?K;
??????xhatminus?=?phi?*?xhatplus;
??????xhatplus?=?xhatminus?+?K?*?（z?-?H?*?xhatminus）;
??????Pplus?=?Pminus?-?K?*?H?*?Pminus;
???elseif?（flag?==?1）
???????%?Use?the?augmented?state?approach
??????Pminus?=?phi1?*?Pplus?*?phi1‘?+?Q1;
??????K?=?inv（H1?*?Pminus?*?H1‘?+?R1）;
??????K?=?（Pminus?*?H1‘）?*?K;
??????xhatminus?=?phi1?*?xhatplus;
??????xhatplus?=?xhatminus?+?K?*?（z?-?H1?*?xhatminus）;
??????Pplus?=?Pminus?-?K?*?H1?*?Pminus;
???elseif?（flag?==?2）
???????%?Use?the?Bryson/Henrikson?approach
??????zeta?=?z?-?psi?*?zold;
??????C?=?M?*?inv（D?*?Pminus?*?D‘?+?R）;
??????K?=?Pminus?*?D‘?*?inv（D?*?Pminus?*?D‘?+?R）;
??????xhatplus?=?xhatminus?+?K?*?（zeta?-?D?*?xhatminus）;
??????xhatminus?=?phi?*?xhatplus?+?C?*?（zeta?-?D?*?xhatplus）;
??????Pplus?=?（eye（2）?-?K?*?D）?*?Pminus?*?（eye（2）?-?K?*?D）‘?+?K?*?R?*?K‘;
??????Pminus?=?phi?*?Pplus?*?phi‘?+?Q?-?C?*?M‘?-?phi?*?K?*?M?-?M‘?*?K‘?*?phi‘;
??????xhatplus?=?xhatminus;
???end
???%?Save?data?for?plotting.
???xArray?=?[xArray?x];
???xhatArray?=?[xhatArray?xhatplus];
???KArray?=?[KArray?K];
???PArray?=?[PArray?Pplus];
???zArray?=?[zArray?z];
end

%?Plot.
k?=?1?:?kf;
close?all;
figure;
plot（k?zArray（1:）?-?xArray（1:）?‘-‘?k?xArray（1:）?-?xhatArray（1:）?‘

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件?????442575??2007-04-28?22:20??OptimalStateEstimation\hinfinity.pdf

?????文件?????435748??2007-04-28?22:23??OptimalStateEstimation\kalman.pdf

?????文件???????3106??2007-04-28?22:18??OptimalStateEstimation\HybridBody.m

?????文件???????4828??2007-04-28?22:15??OptimalStateEstimation\MotorSim.m

?????文件???????5132??2007-04-28?22:18??OptimalStateEstimation\HybridUKF.m

?????文件???????1172??2007-04-28?22:16??OptimalStateEstimation\LinearSimEx1.m

?????文件???????2917??2007-04-28?22:18??OptimalStateEstimation\UnscentedEx.m

?????文件???????1435??2007-04-28?22:17??OptimalStateEstimation\DiscreteKFAlt.m

?????文件???????5816??2007-04-28?22:18??OptimalStateEstimation\ParticleEx2.m

?????文件???????1998??2007-04-28?22:17??OptimalStateEstimation\DiscreteKFEx1.m

?????文件???????3582??2007-04-28?22:18??OptimalStateEstimation\ParticleEx1.m

?????文件???????1840??2007-04-28?22:17??OptimalStateEstimation\DiscreteKFEx2.m

?????文件???????7676??2007-04-28?22:18??OptimalStateEstimation\ParticleEx4.m

?????文件????????899??2007-04-28?22:17??OptimalStateEstimation\DiscreteKFEx2Plot.m

?????文件???????7992??2007-04-28?22:18??OptimalStateEstimation\ParticleEx5.m

?????文件???????3836??2007-04-28?22:17??OptimalStateEstimation\Colored.m

?????文件???????6115??2007-04-28?22:19??OptimalStateEstimation\ParticleEx3.m

?????文件?????231841??2007-04-28?22:21??OptimalStateEstimation\ESDNonlinear.pdf

?????文件???????2849??2007-04-28?22:17??OptimalStateEstimation\Correlated.m

?????文件???????2078??2007-04-28?22:18??OptimalStateEstimation\ContEx.m

?????文件???????7741??2007-04-28?22:18??OptimalStateEstimation\KalmanConstrained.m

?????文件???????5058??2007-04-28?22:18??OptimalStateEstimation\FixPtSmooth.m

?????文件???????3213??2007-04-28?22:18??OptimalStateEstimation\FixLagSmooth.m

?????文件???????3069??2007-04-28?22:18??OptimalStateEstimation\Multiple.m

?????文件???????4192??2007-04-28?22:18??OptimalStateEstimation\FixIntSmooth.m

?????文件???????6755??2007-04-28?22:18??OptimalStateEstimation\Reduced.m

?????文件???????5057??2007-04-28?22:18??OptimalStateEstimation\Robust.m

?????文件???????2095??2007-04-28?22:18??OptimalStateEstimation\Schmidt.m

?????文件???????3000??2007-04-28?22:18??OptimalStateEstimation\ExtendedBody.m

?????文件???????4633??2007-04-28?22:18??OptimalStateEstimation\MotorKalman.m

............此處省略6個文件信息