function?[]?=?ekf_localization（）
????close?all;
????clear?all;

????disp（‘EKF?Start!‘）

????time?=?0;
????global?endTime;?%?[sec]
????endTime?=?60;
????global?dt;
????dt?=?0.1;?%?[sec]

????removeStep?=?5;

????nSteps?=?ceil（（endTime?-?time）/dt）;

????estimation.time=[];
????estimation.u=[];
????estimation.GPS=[];
????estimation.xOdom=[];
????estimation.xEkf=[];
????estimation.xTruth=[];

????%?State?Vector?[x?y?yaw]‘
????xEkf=[0?0?0]‘;
????PxEkf?=?eye（3）;

????%?Ground?True?State
????xTruth=xEkf;

????%?Odometry?Only
????xOdom=xTruth;

????%?Observation?vector?[x?y?yaw]‘
????z=[0?0?0]‘;

????%?Simulation?parameter
????global?noiseQ
????noiseQ?=?diag（[0.1?0?degreeToRadian（10）]）.^2;?%[Vx?Vy?yawrate]

????global?noiseR
????noiseR?=?diag（[0.5?0.5?degreeToRadian（5）]）.^2;%[x?y?yaw]
????
????%?Covariance?Matrix?for?motion
????convQ=eye（3）;
????
????%?Covariance?Matrix?for?observation
????convR=noiseR;

????%?Other?Intial
????xPred?=?[0?0?0]‘;
????F?=?zeros（3）;
????H?=?zeros（3）;
????
????%?Main?loop
????for?i=1?:?nSteps
????????time?=?time?+?dt;
????????%?Input
????????u=robotControl（time）;
????????%?Observation
????????[zxTruthxOdomu]=prepare（xTruth?xOdom?u）;

????????%?------?Kalman?Filter?--------
????????%?Predict
????????xPred?=??doMotion（xEkf?u）;
????????F?=?jacobF（xEkf?u）;
????????convQ?=?F*convQ*F‘+?noiseQ;?????
????????
????????%?Update
????????H?=?jacobH（xPred）;
????????PxEkf?=?convQ*H‘*inv（H*convQ*H‘+convR）;
????????xEkf=?doObservation（z?xPredPxEkf）;
????????convQ=（eye（3）-PxEkf*H）*convQ;?

????????%?-----------------------------
????????%?Simulation?estimation
????????estimation.time=[estimation.time;?time];
????????estimation.xTruth=[estimation.xTruth;?xTruth‘];
????????estimation.xOdom=[estimation.xOdom;?xOdom‘];
????????estimation.xEkf=[estimation.xEkf;xEkf‘];
????????estimation.GPS=[estimation.GPS;?z‘];
????????estimation.u=[estimation.u;?u‘];

????????%?Plot?in?real?time
????????%?Animation?（remove?some?flames）
????????if?rem（iremoveStep）==0
????????????%hold?off;
????????????plot（estimation.GPS（:1）estimation.GPS（:2）‘*m‘?‘MarkerSize‘?5）;hold?on;
????????????plot（estimation.xOdom（:1）estimation.xOdom（:2）‘.k‘?‘MarkerSize‘?10）;hold?on;
????????????plot（estimation.xEkf（:1）estimation.xEkf（:2）‘.r‘‘MarkerSize‘?10）;hold?on;
????????????plot（estimation.xTruth（:1）estimation.xTruth（:2）‘.b‘?‘MarkerSize‘?10）;hold?on;
????????????axis?equal;
????????????grid?on;
????????????drawnow;
????????????%movcount=movcount+1;
????????????%mov（movcount）?=?getframe（gcf）;%?アニメーションのフレームをゲットする
????????end?
????end
????close????
????finalPlot（estimation）;
end

%?control
function?u?=?robotControl（time）
????global?endTime;
????T?=?10;?%?sec
????Vx?=?1.0;?%?m/s
????Vy?=?0.2;?%?m/s
????yawrate?=?5;?%?deg/s
????
????%?half
????if?time?>?（endTime/2）
????????yawrate?=?-5;
????end
????
????u?=[?Vx*（1-exp（-time/T））?Vy*（1-exp（-time