TDOA/AOA定位的扩展卡尔曼滤波定位算法Matlab源码(2007-08-24 01:48:23)标签：知识/探索

function

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

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

%% 此程序为GreenSim团队原创作品，转载请注明

%% 欢迎访问GreenSim团队的主页http://www.doczj.com/doc/f48afe18227916888486d750.html/greensim

%% 欲与程序原作者进行技术交流，请发邮件Email:aihuacheng@http://www.doczj.com/doc/f48afe18227916888486d750.html

%% 输入参数列表

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