%?THIS?PROGRAM?IS?FOR?IMPLEMENTATION?OF?DISCRETE?TIME?PROCESS?EXTENDED?KALMAN?FILTER
%?FOR?GAUSSIAN?AND?LINEAR?STOCHASTIC?DIFFERENCE?EQUATION.
%?By?（R.C.R.C.R）SPLABSMPL.
%?（17?JULY?2005）.
%?Help?by?Aarthi?Nadarajan?is?acknowledged.
%?（drawback?of?EKF?is?when?nonlinearity?is?high?we?can?extend?the
%?approximation?taking?additional?terms?in?Taylor‘s?series）.

clc;??close?all;?clear?all;

Xint_v?=?[1;?0;?0;?0;?0];
wk?=?[1?0?0?0?0];
vk?=?[1?0?0?0?0];

for?ii?=?1:1:length（Xint_v）
????
????Ap（ii）?=?Xint_v（ii）*2;
????W（ii）?=?0;
????H（ii）?=?-sin（Xint_v（i