function X=drop(noi,noise,alp)
rmse=0;
e=[0;0;0;0];
threshold =6; %数据是否有用的阈值
%target trajectory :circle,(x,y)
%theta = 0:0.1:2*pi+0.2;
%N=length(theta);
%x = 0.4*sin(theta)+0.5;
%y = 0.4*cos(theta)+0.5;
%plot(x,y,'-r');hold on;
theta=0:4:100;
N=length(theta);
x=theta;
y=0*theta+20;
plot(x,y,'-r');hold on;
%linearized mode
A = [1 1 0 0;...
0 1 0 0;...
0 0 1 1;...
0 0 0 1];
C = cell(1,N);
%observation data
for k = 1 : N
x(k) = x(k)+0*randn(1);%add system noise ,u(k)
y(k) = y(k)+0*randn(1);
z1(k) = sqrt(x(k)^2+y(k)^2);
z2(k) = sqrt((x(k)-100)^2+y(k)^2);
z3(k) = sqrt(x(k)^2+(y(k)-100)^2);
z4(k) = sqrt((x(k)-100)^2+(y(k)-100)^2);
end
z = cell(1,N);
for k = 1 : N
if k<=45/4||k>65/4
z{k} = [z1(k);z2(k);z3(k);z4(k)] +noise*randn(4,1);%[1;1;1;1];%add observation noise v(k)
else
z{k} = [z1(k);z2(k);z3(k);z4(k)] +noise*randn(4,1)+[0;0;noi+0.1*randn(1);0];
end
end
%estimated state:x_s
x_s = cell(1,N);
x_s{1} = [x(1);0;y(1);0];
P = cell(1,N);
P{1} = eye(4);
Q = 0.01*eye(4);
R = 0.01*eye(4);
x_sTime = cell(1,N);
for k = 1 : N-1
%C{k} = [x_s{k}(1)/sqrt(x_s{k}(1)^2+x_s{k}(3)^2) 0 x_s{k}(3)/sqrt(x_s{k}(1)^2+x_s{k}(3)^2) 0;...
% (2*x_s{k}(1)-2)/sqrt(2*((x_s{k}(1)-1)^2+x_s{k}(3)^2)) 0 x_s{k}(3)/sqrt((x_s{k}(1)-1)^2+x_s{k}(3)^2) 0;...
% x_s{k}(1)/sqrt((x_s{k}(3)-1)^2+x_s{k}(1)^2) 0 (2*x_s{k}(3)-2)/sqrt(2*((x_s{k}(3)-1)^2+x_s{k}(1)^2)) 0];
C{k} = [ 1/(x_s{k}(1)^2+x_s{k}(3)^2)^(1/2)*x_s{k}(1),0, 1/(x_s{k}(1)^2+x_s{k}(3)^2)^(1/2)*x_s{k}(3),0;...
1/2/(x_s{k}(1)^2-200*x_s{k}(1)+10000+x_s{k}(3)^2)^(1/2)*(2*x_s{k}(1)-200), 0, 1/(x_s{k}(1)^2-200*x_s{k}(1)+10000+x_s{k}(3)^2)^(1/2)*x_s{k}(3),0;...
1/(x_s{k}(1)^2+x_s{k}(3)^2-200*x_s{k}(3)+10000)^(1/2)*x_s{k}(1), 0,1/2/(x_s{k}(1)^2+x_s{k}(3)^2-200*x_s{k}(3)+10000)^(1/2)*(2*x_s{k}(3)-200),0;...
1/2/(x_s{k}(1)^2-200*x_s{k}(1)+20000+x_s{k}(3)^2-200*x_s{k}(3))^(1/2)*(2*x_s{k}(1)-200),0, 1/2/(x_s{k}(1)^2-200*x_s{k}(1)+20000+x_s{k}(3)^2-200*x_s{k}(3))^(1/2)*(2*x_s{k}(3)-200),0];
K{k} = P{k}*C{k}'*inv(R+C{k}*P{k}*C{k}');
z_e = [sqrt(x_s{k}(1)^2+x_s{k}(3)^2);sqrt((x_s{k}(1)-100)^2+x_s{k}(3)^2);sqrt(x_s{k}(1)^2+(x_s{k}(3)-100)^2);sqrt((x_s{k}(1)-100)^2+(x_s{k}(3)-100)^2)];
ling=z{k}-z_e;
%e=(e*(k-1)+abs(ling))/k;
for m=1:4
if ling(m)>=3
K{k}(:,m)= 0;
elseif 3>ling(m)&&ling(m)>=0
K{k}(:,m)= K{k}(:,m)/alp;
else
K{k}(:,m)= K{k}(:,m)*alp;
end
end
%if norm(z{k}-z_e)>=threshold % threshold 是否丢弃的阈值,要在程序开始时给出;例如threshold=5;
% K{k}=0;
% else
% K{k} = P{k}*C{k}'*inv(R+C{k}*P{k}*C{k}');
% end
x_sTime{k}=x_s{k}+ K{k}*(z{k}-z_e);%measurement update
x_s{k+1} = A*x_sTime{k};%time update
P{k+1} = A*P{k}*A' + Q - A*P{k}*C{k}'*inv(C{k}*P{k}*C{k}'+R)*C{k}*P{k}*A';
% x_e(k) = x_sTime{k}(1);
% y_e(k) = x_sTime{k}(3);
% plot(x(k),y(k),'-^r');hold on;
% plot(x_e(k),y_e(k),'*k');pause(0.1);hold on;
end
for k = 1 : N-1
x_e(k) = x_sTime{k}(1);
y_e(k) = x_sTime{k}(3);
rmse=rmse+sqrt((x_e(k)-x(k))^2+( y_e(k)-y(k))^2);
end
X=rmse/(N-1);
%X=[x_e',y_e'];
%plot(x_e,y_e,'-b');
%axis([0 100 0 100]);
%xlabel('x');
%ylabel('y');
title('Tracking Performance');
%legend('actual orbit','estimated orbit')
% for k = 41 : 60
% z{k} = [z1(k);z2(k);z3(k);z4(k)] +[1;10;1;1];%add observation noise v(k)
%end
%for k = 61 : N
% z{k} = [z1(k);z2(k);z3(k);z4(k)] +[1;1;1;1];%add observation noise v(k)
%end
没有合适的资源?快使用搜索试试~ 我知道了~
温馨提示
用于室内定位的TDOA算法matlab仿真代码:含chan氏,taylor算法,卡尔曼滤波算法,及改进的基于卡尔曼的奇异值抛弃和整体偏移法,考虑NLOS因素(matlab simulation code of the TDOA algorithm For indoor positioning : containing chan s, taylor algorithm, the Kalman filter algorithm, and improved singular value based on Kalman abandoned and the overall offset method)
资源推荐
资源详情
资源评论
收起资源包目录
TDOA_室内定位_matlab算法_chan_taylor_卡尔曼滤波_NLOS因素.rar (70个子文件)
TDOA_室内定位_matlab算法_chan_taylor_卡尔曼滤波_NLOS因素
TDOA算法matlab仿真代码
有NLOS
轨迹
offset1.m 3KB
chanforline.m 2KB
chan2.asv 3KB
drop1.m 3KB
chanforline.asv 2KB
chan2.m 3KB
line1.m 3KB
compare.m 898B
Taylorforline.m 2KB
compare.asv 890B
offset1.m 3KB
chanforline.m 2KB
drop1.m 3KB
offset.asv 3KB
误差
offset1.m 3KB
drop1.m 3KB
line1.m 3KB
compare.m 873B
Taylorforline.m 2KB
compare.asv 839B
drop.m 3KB
offset1.asv 3KB
line1.m 3KB
test.m 58B
联合法
offset1.m 3KB
test1.m 42B
drop1.m 3KB
line1.m 3KB
compare.m 874B
dropoffset.m 4KB
dropoffset.asv 4KB
compare.m 880B
drop1.asv 3KB
Taylorforline.m 2KB
offset.m 3KB
无NLOS
chan.asv 3KB
轨迹
Taylorforline.asv 2KB
r=2.fig 6KB
chan2.m 3KB
drop.m 3KB
drop.asv 3KB
r=2k.fig 7KB
line1.m 3KB
compare.m 1KB
Taylorforline.m 2KB
compare.asv 839B
compare1.m 1KB
offset.m 3KB
chanforline.m 3KB
chan2.asv 3KB
chanforline.asv 3KB
chan2.m 3KB
chanim.m 3KB
drop.m 3KB
drop.asv 3KB
chan.m 3KB
line1.m 3KB
均方差
chan2.m 3KB
drop.m 3KB
line1.m 3KB
compare.m 871B
testT.m 43B
Taylorforline.m 2KB
compare.asv 778B
offset.m 3KB
compare.m 520B
line1.asv 3KB
Taylorforline.m 2KB
compare.asv 520B
offset.m 3KB
共 70 条
- 1
m0_64795180
- 粉丝: 20
- 资源: 708
下载权益
C知道特权
VIP文章
课程特权
开通VIP
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
- 1
- 2
- 3
- 4
- 5
- 6
前往页