ukf直线跟踪，提供详细代码，误差曲线，详细的代码注解

UKF跟踪，亲测，可用。提供详细代码，误差曲线，详细的代码注解

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  功能说明： UKF在直线定位的应用
%  参数说明： 1、状态3维，x方向的位置；
%               y方向的位置、；航向角；
%             2、观测信息为距离和角度；
%  详细原理介绍及中文注释请参考如下：
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ukf_for_track_6_div_system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n=3;
t=1;
Q=diag（[111]）;
R = diag（[0.01
    0.01/180]）;
f=@（x）[x（1）+10*cos（pi/4）;x（2）+10*sin（pi/4）;pi/4];
h=@（x）[sqrt（x（1）^2+x（2）^2）;atan（x（2）/x（1））];
s=[0;0;pi/4];
x0=s+sqrtm（Q）*randn（n1）;
P0 =eye（3）;
N=200;
Xukf = zeros（nN）;
X = zeros（nN）;
Z = zeros（2N）;
for i=1:N
    X（:i）= f（s）+sqrtm（Q）*randn（31）;% 此处有误，请修改为P114页一致即可运行
    s = X（:i）;
end
ux=x0;
for k=1:N
    Z（:k）= h（X（:k）） + sqrtm（R）*randn（21）;
    [Xukf（:k） P0] = ukf（fuxP0hZ（:k）QR）;
    ux=Xukf（:k）;
end

err_ukf_x=zeros（1N）;
for k=1:N
    err_ukf_x（k）=（X（1k）-Xukf（1k））;
end
ukf_xmax=max（abs（err_ukf_x））;
err_ave_ukf=sum（abs（err_ukf_x））/N;
fprintf（ ‘ukf_xmax=%f‘ukf_xmax）;   
fprintf（‘err_ave_ukf=%f‘err_ave_ukf）;

err_ukf_y=zeros（1N）;
for k=1:N
    err_ukf_y（k）=（X（2k）-Xukf（2k））;
end
ukf_ymax=max（abs（err_ukf_y））;
err_ave_ukf=sum（abs（err_ukf_y））/N;
fprintf（ ‘ukf_ymax=%f‘ukf_ymax） ;   
fprintf（‘err_ave_ukf=%f‘err_ave_ukf）;

err_ukf_t=zeros（1N）;
for k=1:N
    err_ukf_t（k）=（X（3k）-Xukf（3k））;
end
ukf_tmax=max（abs（err_ukf_t））;
err_ave_ukf=sum（abs（err_ukf_t））/N;
fprintf（ ‘ ukf_tmax=%f‘ukf_tmax） ;   
fprintf（‘err_ave_ukf=%f‘err_ave_ukf）;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
t=1:N;
hold on;box on;
plot（ X（1t）X（2t） ‘-k*‘‘LineWidth‘1）
plot（Xukf（1t）Xukf（2t）‘-b+‘‘LineWidth‘1）
grid on;
legend（‘真实轨迹‘‘UKF轨迹‘）;
xlabel（‘x方向位置/cm‘）
ylabel（‘y方向位置/cm‘）

figure
box on;
plot（err_ukf_x‘-b*‘‘LineWidth‘1）
grid on;
xlabel（‘时间t/秒‘）
ylabel（‘x方向位置误差/cm‘）

figure
box on;
plot（err_ukf_y‘-r*‘‘LineWidth‘1）
grid on;
xlabel（‘时间t/秒‘）
ylabel（‘y方向位置误差/cm‘）

figure
box on;
plot（err_ukf_t‘-m‘‘LineWidth‘1）
grid on;
xlabel（‘时间t/秒‘）
ylabel（‘航向角误差/°‘）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [XP]=ukf（ffunXPhfunZQR）
L=numel（X）;
m=numel（Z）;
alpha=1e-2;
ki=0;
beta=2;
lambda=alpha^2*（L+ki）-L;
c=L+lambda;
Wm=[lambda/c 0.5/c+zeros（12*L）];
Wc=Wm;
Wc（1）=Wc（1）+（1-alpha^2+beta）;
c=sqrt（c）;
Xsigmaset=sigmas（XPc）; 
[X1meansX1P1X2]=ut（ffunXsigmasetWmWcLQ）;   
[ZpreZ1PzzZ2]=ut（hfunX1WmWcmR）;
Pxz=X2*diag（Wc）*Z2‘;
K=Pxz*inv（Pzz）;
X=X1means+K*（Z-Zpre）;
P=P1-K*Pxz‘;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [XmeansXsigma_prePXdiv]=ut（funXsigmaWmWcnCOV）
LL=size（Xsigma2）;
Xmeans=zeros（n1）;
Xsigma_pre=zeros（nLL）;
for k=1:LL
    Xsigma_pre（:k）=fun（Xsigma（:k））;
    Xmeans=Xmeans+Wm（k）*Xsigma_pre（:k）;
end
Xdiv=Xsigma_pre-Xmeans（:ones（1LL

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        3406  2018-05-04 16:15  ukf_for_track_6_div_system.m