【目标跟踪-卡尔曼滤波】基于分布式Kalman滤波跟踪运动目标附Matlab代码(单目标跟踪算法)

近年来，随着工业的发展，对于一些实际模型的建立所提出的要求也越来越高。作为对传统整数阶微积分学的一种扩展，分数阶微积分能实现对实际系统的更为准确的描述。目前，分数阶微积分在冶金、化工、电力、轻工和机械等工业过程中都有着广泛的应用，它的发展也为各个学科的发展提供了新的理论基础。

SISO分数阶系统的分数卡尔曼滤波器算法源代码，包括：分数扩展卡尔曼滤波器（FEKF）、分数中心卡尔曼滤波器（FCDKF）、分数无迹卡尔曼滤波器（FUKF）和分数粒子滤波器（FPF）

%*************************************************************************%

% 无迹卡尔曼滤波器仿真复现 %

%_________________________________________________________________________%

% 论文 : 非线性系统滤波理论 P108 5.4.2 强非线性系统模型仿真

% 目的 : 无迹卡尔曼滤波器仿真复现

% 函数实验 ::

% | 3*sin(2*x_2) | | 1 |

% x_k = | x_1 + exp(-0.05x_3) + 10 | + | 1 |w_k

% | = x_1(x_2 + x_3)/5 + |x_1|/2 | | 1 |

%

% 结果 : 滤波效果良好

%

% 备注 :

%

%*************************************************************************%

clc

clear

%仿真步长

N = 50;

q = 0.3; %系统噪声均值

r = 0.5; %测量噪声均值

Q = 0.7; %系统噪声方差矩阵

R = 1.0; %测量噪声方差矩阵

% %GL定义下短记忆原理的长度

% L = N+1;

% %计算alpha阶次对应的GL定义系数 binomial coefficient

% bino_fir = zeros(1,N); %微分阶次为0.7时GL定义下的系数

% alpha = 0.7;

% bino_fir(1,1) = 1;

% for i = 2:1:N

% bino_fir(1,i) = (1-(alpha+1)/(i-1))*bino_fir(1,i-1);

% end

I = eye(3,3); %生成单位阵

%err_state_FEKF = zeros(kk_N,N);

X_state_real = zeros(3,N); %真实状态

Z_state_meas = zeros(1,N); %实际观测值

%噪声

W_noise = sqrt(Q)*randn(1,N) + q; %系统噪声

V_noise = sqrt(R)*randn(1,N) + r; %测量噪声

x_0 = [-0.7; 1; 1]; %初始状态

X_state_real(:,1) = x_0; %真实状态初始值

Z_state_meas(1,1) = V_noise(1,1); %测量数据初始值

f=@(x)[3*sin(2*x(2)); ...

x(1) + exp(-0.05*x(3)) + 10; ...

0.2 * x(1) * (x(2) + x(3)) + 0.5*abs(x(1)) ];

h=@(x)x(1) + x(2) * x(3);

for k=2:1:N

%计算实际状态

X_state_real(:,k) = f(X_state_real(:,k-1)) + [1; 1; 1] * W_noise(1,k-1);

%实际观测值

Z_state_meas(1,k) = h(X_state_real(:,k)) + V_noise(1,k);

end

%*************************************************************************%

%-------------------------无迹卡尔曼滤波器性能测试------------------------%

%*************************************************************************%

X_state_esti = zeros(3,N); %状态最优估计值

P_xesti = cell(1,N); %估计误差方差阵

%初始值设置（初始矩阵不能为零）

P_pred_0 = eye(3,3); %初始预测方差阵

P_xesti{1,1} = P_pred_0; %初始估计方差阵

state_dim = 3;

L_sample = 2 * state_dim +1;

SigmaPoints = zeros(3, L_sample);

SigmaWeight = zeros(1, L_sample);

GammaPoints = zeros(3, L_sample);

ChiPoints = zeros(1, L_sample);

%*******************************%

% 画图输出 均值方差估计散点图

%*******************************%

%输入与测量输出图

k = 1:1:N;

LineWidth = 1.5;

%square error

figure;

plot(k,X_state_real(3,:),'r',k,X_state_esti(3,:),'b--','linewidth',LineWidth);

%set(gcf,'Position',[200 200 400 300]);

%axis([xmin xmax ymin ymax])设置坐标轴在指定的区间

% axis normal

% axis([ -10 N 0 6 ])

ylabel('$x_3$','FontSize',8)

xlabel('time(sec)','FontSize',8)

%设置坐标轴刻度字体名称，大小

set(gca,'FontName','Helvetica','FontSize',8)

legend('real state','estimated state','Location','best');

legend('Real state 3','Estimation state 3','Location','best');

figure;

plot(k,X_state_real(2,:),'r',k,X_state_esti(2,:),'b--','linewidth',LineWidth);

%set(gcf,'Position',[200 200 400 300]);

%axis([xmin xmax ymin ymax])设置坐标轴在指定的区间

% axis normal

% axis([ -10 N 0 6 ])

ylabel('$x_3$','FontSize',8)

xlabel('time(sec)','FontSize',8)

%设置坐标轴刻度字体名称，大小

set(gca,'FontName','Helvetica','FontSize',8)

legend('real state','estimated state','Location','best');

legend('Real state 2','Estimation state 2','Location','best');

figure;

plot(k,X_state_real(1,:),'r',k,X_state_esti(1,:),'b--','linewidth',LineWidth);

%set(gcf,'Position',[200 200 400 300]);

%axis([xmin xmax ymin ymax])设置坐标轴在指定的区间

% axis normal

% axis([ -10 N 0 6 ])

ylabel('$x_3$','FontSize',8)

xlabel('time(sec)','FontSize',8)

%设置坐标轴刻度字体名称，大小

set(gca,'FontName','Helvetica','FontSize',8)

legend('real state','estimated state','Location','best');

legend('Real state 1','Estimation state 1','Location','best');

[1]陈林. 一种基于卡尔曼滤波的运动目标跟踪方法[J]. 舰船电子对抗, 2011, 034(003):67-70.

[2]王雯洁, 汪非易, 赵航芳. 基于分布式传感器能量比的水下运动目标扩展卡尔曼滤波跟踪方法:, 2018.