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

TDOA/AOA定位是无线定位领域里使用得最多的一种定位体制，扩展卡尔曼滤波器是最经典的非线性滤波算法，可用于目标的定位和动态轨迹跟踪，GreenSim团队实现了该算法，本源码由GreenSim团队原创，转载请注明，有意购买源码或代写相关程序，请与GreenSim团队联系：[email=greensim@163]greensim@163[/email].com

function [MX,MY,SS]=ExtendedKalmanFilter(D1,D2,D3,A1,A2,A3,Flag1,FLAG2,S0,P0,SigmaR,SigmaAOA)

%% TDOA/AOA定位的扩展卡尔曼滤波定位算法

% 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;

el