【wsn定位问题】基于chan算法求解wsn定位问题matlab源码

1 模型介绍

模型参考这里。

2 部分代码

function X = ChanAlgorithm(BSN, MSP, Radius, Noise)
%CHANALGORITHM 本函数用于实现无线定位中的CHAN算法
%               - BSN  为基站个数，3 < BSN <= 7；
%               - MSP  为移动台的初始位置, MSx, MSy均为[0,1]之间的数；
%                      特别要注意服务小区与MS之间的关系，MS的位置不能越界。
%               - Noise 测距误差方差。
%               - R    为小区半径，单位(meter)；
%               - X    为移动台经算法处理后的位置.
%See also: ChanAlgorithm.m
 
 
 
%   参数检查：
if  nargout>1,
    error('Too many output arguments.');
end
if nargin<2 | nargin>4,
    error('Wrong number of input arguments.');
end
 
 
% 算法开始：
BS = Radius*NetworkTop(BSN);
MS = Radius*MSP;
 
% 噪声功率：
Q = eye(BSN-1);
 
% 第一次LS：
% Ri
K1 = 0;
for i = 1: BSN,
    R0(i) = sqrt((BS(1,i) - MS(1))^2 + (BS(2,i) - MS(2))^2);
end
 
for i = 1: BSN-1,
    R(i) = R0(i+1) - R0(1) + Noise*randn(1);
    K(i) = BS(1,i+1)^2 + BS(2,i+1)^2;
end
 
% Ga
for i = 1: BSN-1,
    Ga(i,1) = -BS(1, i+1);
    Ga(i,2) = -BS(2, i+1);
    Ga(i,3) = -R(i);
end
 
% h
for i = 1: BSN-1,
    h(i) = 0.5*(R(i)^2 - K(i) + K1);
end
 
% 由（14b）给出B的估计值：
Za0 = inv(Ga'*inv(Q)*Ga)*Ga'*inv(Q)*h';
 
% 利用这个粗略估计值计算B：
B = eye(BSN-1);
for i = 1: BSN-1,
    B(i,i) = sqrt((BS(1,i+1) - Za0(1))^2 + (BS(2,i+1) - Za0(2))^2);
end
 
% FI:
FI = B*Q*B;
 
% 第一次LS结果：
Za1 = inv(Ga'*inv(FI)*Ga)*Ga'*inv(FI)*h';
 
if Za1(3) < 0,
    Za1(3) = abs(Za1(3));
%     Za1(3) = 0;
end
%***************************************************************
 
% 第二次LS：
% 第一次LS结果的协方差：
CovZa = inv(Ga'*inv(FI)*Ga);
 
% sB：
sB = eye(3);
for i = 1: 3,
    sB(i,i) = Za1(i);
end
 
% sFI：
sFI = 4*sB*CovZa*sB;
 
% sGa：
sGa = [1, 0; 0, 1; 1, 1];
 
% sh
sh  = [Za1(1)^2; Za1(2)^2; Za1(3)^2];
 
% 第二次LS结果：
Za2 = inv(sGa'*inv(sFI)*sGa)*sGa'*inv(sFI)*sh;
 
% Za = sqrt(abs(Za2));
 
Za = sqrt(Za2);
 
% 输出:
% if Za1(1) < 0,
%     out1 = -Za(1);
% else
%     out1 = Za(1);
% end
% if Za2(1) < 0,
%     out2 = -Za(2);
% else
%     out2 = Za(2);
% end

3 仿真结果

4 参考文献

[1]章坚武, 唐兵, 秦峰. Chan定位算法在三维空间定位中的应用[J]. 计算机仿真, 2009, 26(001):323-326.

标签：end,定位问题,inv,wsn,BSN,源码,Ga,BS,Za1
来源： https://blog.csdn.net/Matlab_dashi/article/details/120516534