核心提示：MATLAB代做|FPGA代做|python代做- 室内跟踪-pf...
clc;
clear;
close all;
obs=23;
T=1;
v=1;
line_x=ones(1,23);
line_x=line_x*8.63;
line_y=(1:obs)*T*v;
line_track=[line_x;line_y];
%plot(line_x,line_y);
line_distance=zeros(3,23);
for n=1:23
line_distance(1,n)=sample_distance(line_x(n),line_y(n),1);
line_distance(2,n)=sample_distance(line_x(n),line_y(n),2);
line_distance(3,n)=sample_distance(line_x(n),line_y(n),3);
end
%line_distance
line_signal=zeros(3,23);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%测到的理论上的直线轨迹信号值
for n=1:3
for m=1:23
line_signal(n,m)=indoor_path_loss_modle(line_distance(n,m));
end
end
%%%%%%%%%%%%%%%%%%%%
measured_noise=normrnd(4,64,3,23);%%%%%%%%%%加上测量误差得到测量值
%measured_noise=repmat(measured_noise,3,1);
line_signal=line_signal+measured_noise;
S1_location=signal_data_base(line_signal,23,7);
S2_location=signal_data_base(line_signal,23,4);
line_error=S1_location-line_track;
S1_error=sqrt(line_error(1,:).^2+line_error(2,:).^2);
% plot(location_error);
mean(S1_error)
std(S1_error)
%%%%%%%%%%%%%%%运用卡尔曼滤波
%X(k)=AX(k-1)+N(k)
%Z(k)=HX(k)+R(K)
X=[8.63;0;0;1];
Z=S1_location;
A=[1 0 0 0;0 1 0 1;0 0 1 0;0 0 0 1];
Q=[1/2 0; 0 1/2;1 0;0 1]*[1 0;0 1]*[1/2 0; 0 1/2;1 0;0 1].';
H=[1 0 0 0;0 1 0 0];
R=[25,0;0,25];
P=zeros(4,4);
Xk=[X zeros(4,obs)];
K_location=KF(A,Xk,Z,Q,H,R,P,obs);
Kline_error=K_location(1:2,2:end)-line_track;
K_error=sqrt(Kline_error(1,:).^2+Kline_error(2,:).^2);
figure;
plot(K_error);
hold on
plot(S1_error,'r');
hold off;
mean(K_error)
std(K_error)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%粒子滤波
N=150;
P_location=zeros(4,obs);
xk=zeros(4,N);
wk=zeros(4,N);
v=2.5*randn(1,N);
V=zeros(4,N);
V(2,:)=V(2,:)+v;
for n=1:N
xk(:,n)=X+V(:,n);
end
%初始化粒子和权重
%xk=repmat(X,1,N);
%xk(2,:)=xk(2,:)+v;
wk=1/N*ones(4,N);
for n=1:obs
v=2*randn(1,N);
V=zeros(4,N);
V(2,:)=V(2,:)+v;
[xk,wk]=pf(xk,wk,N,A,V,Z(:,n));
P_location(:,n)=sum(xk.*wk,2);
end
Pline_error=P_location(1:2,:)-line_track;
P_error=sqrt(Pline_error(1,:).^2+Pline_error(2,:).^2);
figure;
plot(K_error);
hold on
plot(S1_error,'r');
hold on
plot(P_error,'g')
hold off;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%RSS+MEMS粒子滤波
% xkM=zeros(2,N);%粒子初始化
% xkM=repmat(X(1:2),1,N);
% wkM=zeros(2,N);%权值初始化
% wkM=1/N*ones(2,N);
% %%%%%%%%%%%%%%%%%%%%%%map
% xkM2=zeros(2,N);%粒子初始化
% xkM2=repmat(X(1:2),1,N);
% wkM2=zeros(2,N);%权值初始化
% wkM2=1/N*ones(2,N);
% for n=1:obs
% step_number=2;
% step_size=0.5;
% delta=0.25;
% alpha_m=pi/2+unifrnd(0,5*pi/180,1,N);
% Distance_k=normrnd(step_number*step_size,0.9,1,N);
% [xkM,wkM]=pfM(xkM,wkM,N,Distance_k,alpha_m,Z(:,n));
% [xkM2,wkM2]=pfM(xkM2,wkM2,N,Distance_k,alpha_m,Z(:,n));
% MP_location(:,n)=sum(xkM.*wkM,2);
% M2P_location(:,n)=sum(xkM2.*wkM2,2);
% end
% MPline_error=MP_location(1:2,:)-line_track;
% MP_error=sqrt(MPline_error(1,:).^2+MPline_error(2,:).^2);
% M2Pline_error=M2P_location(1:2,:)-line_track;
% M2P_error=sqrt(M2Pline_error(1,:).^2+M2Pline_error(2,:).^2);
% figure;
% plot(K_error);
% hold on
% plot(S1_error,'.');
% hold on
% plot(MP_error,'r');
% hold on
% plot(P_error,'g')
% hold on;
% plot(M2P_error,'>')
% hold off;
% mean(P_error)
% std(P_error)
% mean(MP_error)
% std(MP_error)
% mean(M2P_error)
% std(M2P_error)
联系：highspeedlogic
QQ ：1224848052
微信：HuangL1121
邮箱：1224848052@qq.com
微信扫一扫：
--------------------------------------------------------------------------------------
人工智能代做，深度学习代做，深度强化学习代做，zynq智能系统FPGA开发，
AI代做，卷积神经网络，Alexnet，GoogleNet，CNN，TensorFlow，
caffe，pointnet，PPO，Qlearning，FasterRCNN，MTCNN，
SPPNet，word2vec，SARASA算法，梯度策略等等
