TDOA,AOA定位的扩展卡尔曼滤波定位算法Matlab源码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定位的扩展卡尔曼滤波定位算法
%% 输入参数列表
% D1 基站1和移动台之间的距离
% D2 基站2和移动台之间的距离
% D3 基站3和移动台之间的距离
% A1 基站1测得的角度值
% A2 基站2测得的角度值
% A3 基站3测得的角度值
1×1矩阵,取值1,2,3,表明是以哪一个基站作为基准站计算TDOA数据的 % Falg1
% FLAG2 N×3矩阵,取值0和1,每一行表示该时刻各基站的AOA是否可选
择, % 1表示选择AOA数据,FLAG2并非人为给定,而是由LOS/NLOS检测模块确
定
% S0 初始状态向量,4×1矩阵
% P0 预测误差矩阵的初始值,4×4的矩阵