function?EKF4（）????
%???EKF??static?base?alignment
%????ZMH
????clear?all

????L?=?34*pi/180;
????wie?=?7.2921158e-5;
????g?=?9.8;
????sl?=?sin（L）;
????cl?=?cos（L）;
????
????gyrobias?=?0.02*pi/180/3600;
????accbias?=?1e-4*9.8;
????gyrostoc?=?0.01*pi/180/3600;
????accstoc=?0.5e-4*9.8/2;

????P?=?diag（[0.1^20.1^2（1*pi/180）^2（1*pi/180）^2（1*pi/180）^2]）;


????sQ?=?diag（[accstocaccstocgyrostocgyrostocgyrostoc]）;
????Q?=?diag（[accstoc^2accstoc^2gyrostoc^2gyrostoc^2gyrostoc^2]）;
????sR?=?diag（[0.10.1]）;
????R?=?diag（[0.010.01]）;
????
????H?=?[diag（[11]）zeros（23）];

????xest?=?[0.10.1（1*pi/180）（1*pi/180）（1*pi/180）]‘;????%estimate
????x?=?zeros（51）;????dxpre?=?zeros（51）;dxest?=?zeros（51）;

????xobs?=?zeros（51）;
????dxobs?=?multivariate_gauss（x?Q?1）;
????z?=?H*dxobs;
????dzobs?=?multivariate_gauss（z?R?1）;


NN?=?1000;



for?i=1:NN
????
????F?=eye（5）?+?numerical_Jacobian（xobs?@predict_model）;

????dxpre?=?F*dxest;
????P?=?F*P*F‘?+?Q;
???
????%?update
????[dxestP]?=?update（dxpre?P?dzobs?R?H?3）;?%?the?end?parameter?specifies?which?KF?function?to?use.
????

????xest?=?xobs?+?dxest;
????
????dxobs?=?F*dxobs;
????dxobs?=?multivariate_gauss（dxobs??Q?1）;
????xobs?=?xobs?+?predict_model（xobs）;
????

????dz?=?observe_model（dxobsH）;
	dzobs?=?multivariate_gauss（dz?R?1）;??

????%?plots
????xout（:i）?=?xest;
????Pout（1i）?=?sqrt（P（33））;
????Pout（2i）?=?sqrt（P（44））;
????Pout（3i）?=?sqrt（P（55））;
end

????i=1:NN;
????subplot（311）
????plot（ixout（1i）‘r‘ixout（2i）‘b‘）;
????????hold?on
????subplot（312）
????plot（ixout（3i）‘r‘ixout（4i）‘b‘ixout（5i）‘g‘）
????????hold?on
????subplot（313）
????plot（iPout（1i）‘r‘iPout（2i）‘b‘iPout（3i）‘g‘）
????????hold?on