扩展卡尔曼滤波程序（matlab）

t=3;
Ts=0.1;
len=fix（t/Ts）;

xn=zeros（3len）;
x_pre=zeros（3len）;
x_up=zeros（3len）;

s_con=zeros（31）;
s_con（11）=9.8*cos（pi/6）;
s_con（21）=9.8*sin（pi/6）;
s_con（31）=0;

xn（31）=pi/6;
x_pre（31）=pi/6;

z=zeros（2len）;
h=zeros（2len）;

sx=0.1;
sy=0.1;
sa=pi/1800;

dafa=pi/720;
dr=6;

Qk=diag（[sa;sx;sy]）^2;
Rk=diag（[dr;dafa]）^2;

p=1*eye（3）;

F=zeros（33）;
F（11）=1;
F（22）=1;
F（33）=1;

for i=1:len-1
%真实值
xn（1i+1）=xn（1i）+s_con（11）+sx*randn（11）;
xn（2i+1）=xn（2i）+s_con（21）+sy*randn（11）;
xn（3i+1）=xn（3i）+sa*randn（11）;
%测量值
z（1i+1）=sqrt（（xn（1i+1））^2+（xn（2i+1））^2）+dr*randn（11）;
z（2i+1）=atan（xn（2i+1）/xn（1i+1））+dafa*randn（11）;

%预测值
x_pre（1i+1）=x_up（1i）+s_con（11）;
x_pre（2i