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

function [MXMYSS]=ExtendedKalmanFilter（D1D2D3A1A2A3Flag1FLAG2S0P0SigmaRSigmaAOA）
%% 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矩阵，取值123，表明是以哪一个基站作为基准站计算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输入有误，它只能取123‘）;
end
%% 第二步：构造两个固定的矩阵
%构造状态转移矩阵Φ
Phi=[1    00.025    0;
     0    1    00.025;
     0    0    1    0;
     0    0    0    1];
%构造W的协方差矩阵Q
SigmaU=0.00001;%噪声方差取很小的值
Q=[0     0     0     0;
   0     0     0     0;
   0     0SigmaU     0;
   0     0     0SigmaU];
%% 第三步：输出数据初始化
N=length（D1）;
MX=zeros（1N）;
MY=zeros（1N）;
MX（1）=S0（1）;
MY（1）=S0（2）;
SS=zeros（4N）;
SS（:1）=S0;
%% 第四步：以下是迭代过程
for i=2:N
    Flag2=FLAG2（i:）;%当前各信道环境的LOS/NLOS判据
    R=FunR（SigmaRSigmaAOAFlag2）;%调用产生R矩阵的子函数
    S1=Phi*S0;%由状态方程得到的预测值
    H=FunH（S1Flag1Flag2）;%调用产生H矩阵的子函数
    P1=Phi*P0*（Phi‘）+Q;%计算上述预测值的协方差矩阵
    K=P1*（H‘）*inv（H*P1*（H‘）+R）;%计算滤波增益（加权系数）
    Z=FunZ（TDOA1TDOA2A1A2A3SigmaRSigmaAOAFlag2i）;%调用构造观察向量的子函数
    hS1=FunhS1（S1Flag1Flag2）;%调用构造观测值的估计值向量的子函数
    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（TDOA1TDOA2A1A2A3SigmaRSigmaAOAFlag2i）
%调用构造观察向量的子函数
m=sum（Flag2）;
Z=zeros（2+m1）;
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&&F