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+1）=x_up（2i）+s_con（21）;
x_pre（3i+1）=x_up（3i）;

h（1i+1）=sqrt（（x_pre（1i+1））^2+（x_pre（2i+1））^2）;
h（2i+1）=atan（x_pre（2i+1）/x_pre（1i+1））;

p_pre=F*p*F‘+Qk;

%觀測矩陣的雅克比矩陣
H=zeros（23）;
r=sqrt（（x_pre（1i+1））^2+（x_pre（2i+1））^2）;
H（11）=x_pre（1i+1）/r;
H（12）=x_pre（2i+1）/r;
H（13）=0;
H（21）=-x_pre（2i+1）/r^2;
H（22）=x_pre（1i+1）/r^2;
H（23）=0;

%卡爾曼增益
S=H*p_pre*H‘+Rk;
K=p_pre*H‘*（S^-1）;

%更新方程
x_up=x_pre+K*（z-h）;
p=p_pre-K*H*p_pre;

end

x_up（11）=0;
x_up（21）=-20;


figurehold?ongrid?on;
plot（xn（1:）xn（2:）‘-b‘）;
plot（z（1:）.*cos（z（2:））z（1:）.*sin（z（2:））‘G‘）;
plot（x_up（1:）x_up（2:）‘r‘）;
xlabel（‘X‘）;
ylabel（‘Y‘）;
title（‘EKFsimulation‘）;
legend（‘real‘‘measurement‘‘ekfestimated‘）;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????1510??2019-12-31?21:55??EKF.m