03TDOA_AOA定位的扩展卡尔曼滤波算法MATLAB源代码.docxVIP

TDOA/AOA定位的扩展卡尔曼滤波算法MATLAB源代码TDOA/AOA是无线定位领域里使用得比较多的一种定位体制，扩展卡尔曼滤波器是最经典的非线性滤波算法，可用于目标的定位和动态轨迹跟踪。function [MX,MY,SS]=ExtendedKalmanFilter(D1,D2,D3,A1,A2,A3,Flag1,FLAG2,S0,P0,SigmaR,SigmaAOA)%% TDOA/AOA定位的扩展卡尔曼滤波定位算法% GreenSim团队——专业级算法设计代写程序% 欢迎访问GreenSim团队主页→/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的协方差矩阵QSigmaU=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;endfunction 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)==0Flag2(2)==0Flag2(3)==0 %空语句elseif Flag2(1)==1Flag2(2)==0