TDOA/AOA定位的扩展卡尔曼滤波定位跟踪算法Matlab源码

aiwa

TDOA/AOA定位是无线定位领域里使用得最多的一种定位体制，扩展卡尔曼滤波器是最经典的非线性滤波算法，可用于目标的定位和动态轨迹跟踪，GreenSim团队实现了该算法，本源码由GreenSim团队原创，转载请注明，有意购买源码或代写相关程序，请与GreenSim团队联系（主页http://blog.sina.com.cn/greensim）
function [MX,MY,SS]=ExtendedKalmanFilter(D1,D2,D3,A1,A2,A3,Flag1,FLAG2,S0,P0,SigmaR,SigmaAOA)
%% TDOA/AOA定位的扩展卡尔曼滤波定位算法
% GreenSim团队原创作品，转载请注明
% 更多原创作品，请访问GreenSim团队主页→http://blog.sina.com.cn/greensim
%% 输入参数列表
%  D1        基站1和移动台之间的距离
%  D2        基站2和移动台之间的距离
%  D3        基站3和移动台之间的距离
%  A1        基站1测得的角度值
%  A2        基站2测得的角度值
%  A3        基站3测得的角度值
%  Falg1     1×1矩阵，取值1,2,3，表明是以哪一个基站作为基准站计算TDOA数据的
%  FLAG2     N×3矩阵，取值0和1，每一行表示该时刻各基站的AOA是否可选择，
%            1表示选择AOA数据，FLAG2并非人为给定，而是由LOS/NLOS检测模块确定
%  S0        初始状态向量，4×1矩阵
%  P0        预测误差矩阵的初始值，4×4的矩阵
%  SigmaR    无偏/有偏卡尔曼输出距离值的方差，由事先统计得到
%  SigmaAOA  选择AOA数据的方差，生成AOA数据的规律已知，因此可以确定
%%  输出参数列表
%  MX
%  MY

%% 第一步：计算TDOA数据
if Flag1==1
    TDOA1=D2-D1;
    TDOA2=D3-D1;
elseif Flag1==2
    TDOA1=D1-D2;
    TDOA2=D3-D2;
elseif Flag1==3
    TDOA1=D1-D3;
    TDOA2=D2-D3;
else
    error('Flag1输入有误，它只能取1,2,3');
end

%% 第二步：构造两个固定的矩阵
%构造状态转移矩阵Φ
Phi=[1,    0,0.025,    0;
     0,    1,    0,0.025;
     0,    0,    1,    0;
     0,    0,    0,    1];
%构造W的协方差矩阵Q
SigmaU=0.00001;%噪声方差取很小的值
Q=[0,     0,     0,     0;
   0,     0,     0,     0;
   0,     0,SigmaU,     0;
   0,     0,     0,SigmaU];

%% 第三步：输出数据初始化
N=length(D1);
MX=zeros(1,N);
MY=zeros(1,N);
MX(1)=S0(1);
MY(1)=S0(2);
SS=zeros(4,N);
SS(:,1)=S0;

%% 第四步：以下是迭代过程
for i=2:N
    Flag2=FLAG2(i,;%当前各信道环境的LOS/NLOS判据
    R=FunR(SigmaR,SigmaAOA,Flag2);%调用产生R矩阵的子函数
    S1=Phi*S0;%由状态方程得到的预测值
    H=FunH(S1,Flag1,Flag2);%调用产生H矩阵的子函数
    P1=Phi*P0*(Phi')+Q;%计算上述预测值的协方差矩阵
    K=P1*(H')*inv(H*P1*(H')+R);%计算滤波增益（加权系数）
    Z=FunZ(TDOA1,TDOA2,A1,A2,A3,SigmaR,SigmaAOA,Flag2,i);%调用构造观察向量的子函数
    hS1=FunhS1(S1,Flag1,Flag2);%调用构造观测值的估计值向量的子函数
    S2=S1+K*(Z-hS1);%加权得到滤波输出值
    %更新S0和P0
    P2=P1-K*H*P1;
    S0=S2;
    P0=P2;
    %记录滤波输出值
    MX(i)=S2(1);
    MY(i)=S2(2);
    SS(:,i)=S2;
end


function Z=FunZ(TDOA1,TDOA2,A1,A2,A3,SigmaR,SigmaAOA,Flag2,i)
%调用构造观察向量的子函数
m=sum(Flag2);
Z=zeros(2+m,1);
Z(1)=TDOA1(i);
Z(2)=TDOA2(i);
if Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==0
    %空语句
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==0
    Z(3)=A1(i);
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==0
    Z(3)=A2(i);
elseif Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==1
    Z(3)=A3(i);
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==0
    Z(3)=A1(i);
    Z(4)=A2(i);
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==1
    Z(3)=A1(i);
    Z(4)=A3(i);
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==1
    Z(3)=A2(i);
    Z(4)=A3(i);
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==1
    Z(3)=A1(i);
    Z(4)=A2(i);
    Z(5)=A3(i);
else
    error('Flag2格式不正确，它的元素只能取0或者1');
end


function R=FunR(SigmaR,SigmaAOA,Flag2)
%% 产生R矩阵的子函数
m=sum(Flag2);
B=[-1,1,0;
   -1,0,1];
R11=B*[SigmaR,0,0;0,SigmaR,0;0,0,SigmaR]*(B');
R12=zeros(2,m);
R21=zeros(m,2);
if m==0
    R22=[];
elseif m==1
    R22=SigmaAOA;
elseif m==2
    R22=[SigmaAOA,       0;
                0,SigmaAOA];
elseif m==3
    R22=[SigmaAOA,       0,       0;
                0,SigmaAOA,       0;
                0,       0,SigmaAOA];
else
    error('Flag2格式不正确，它的元素只能取0或者1');
end
R=[R11,R12;
   R21,R22];


function hS1=FunhS1(S1,Flag1,Flag2)
%% 构造观测值的估计值向量的子函数
%提取估计的移动台坐标
x=S1(1);
y=S1(2);
%三个基站的横纵坐标
x1=0;
y1=0;
x2=5;
y2=8.66;
x3=10;
y3=0;
%计算移动台到三个基站的距离（估计值）
d1=((x-x1)^2+(y-y1)^2)^0.5;
d2=((x-x2)^2+(y-y2)^2)^0.5;
d3=((x-x3)^2+(y-y3)^2)^0.5;
M=2+sum(Flag2);
hS1=zeros(M,1);
if Flag1==1%以第一个基站为基准计算TDOA数据
    hS1(1)=d2-d1;
    hS1(2)=d3-d1;
elseif Flag1==2%以第二个基站为基准计算TDOA数据
    hS1(1)=d1-d2;
    hS1(2)=d3-d2;
elseif Flag1==3%以第三个基站为基准计算TDOA数据
    hS1(1)=d1-d3;
    hS1(2)=d2-d3;
else
    error('Flag1格式不正确，它只能取1,2,3');
end

h1=atan2(y-y1,x-x1);
h2=atan2(y-y2,x-x2);
h3=atan2(y-y3,x-x3);
if Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==0
    %空语句
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==0
    hS1(3)=h1;
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==0
    hS1(3)=h2;
elseif Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==1
    hS1(3)=h3;
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==0
    hS1(3:4)=[h1;h2];
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==1
    hS1(3:4)=[h1;h3];
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==1
    hS1(3:4)=[h2;h3];
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==1
    hS1(3:5)=[h1;h2;h3];
else
    error('Flag2格式不正确，它的元素只能取0或者1');
end
专业级算法设计&代写程序，请加GreenSim团队QQ：761222791