function [MX,MY,SS]=ExtendedKalmanFilter(D1,D2,D3,A1,A2,A3,Flag1,FLAG2,S0,P0,SigmaR,SigmaAOA)
%% TDOA/AOA定位的扩展卡尔曼滤波定位算法
%% 输入参数列表
% 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的协方差矩阵Q
SigmaU=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;
end
function 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)==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&&Flag2(3)==0
Z(3)=A2(i);
elseif Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==1
Z(3)=A3(i);
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==0
Z(3)=A1(i);
Z(4)=A2(i);
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==1
Z(3)=A1(i);
Z(4)=A3(i);
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==1
Z(3)=A2(i);
Z(4)=A3(i);
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==1
Z(3)=A1(i);
Z(4)=A2(i);
Z(5)=A3(i);
else
error('Flag2格式不正确，它的元素只能取0或者1');
end
function R=FunR(SigmaR,SigmaAOA,Flag2)
%% 产生R矩阵的子函数
m=sum(Flag2);
B=[-1,1,0;
-1,0,1];
R11=B*[SigmaR,0,0;0,SigmaR,0;0,0,SigmaR]*(B');
R12=zeros(2,m);
R21=zeros(m,2);
if m==0
R22=[];
elseif m==1
R22=SigmaAOA;
elseif m==2
R22=[SigmaAOA, 0;
0,SigmaAOA];
elseif m==3
R22=[SigmaAOA, 0, 0;
0,SigmaAOA, 0;
0, 0,SigmaAOA];
else
error('Flag2格式不正确，它的元素只能取0或者1');
end
R=[R11,R12;
R21,R22];
function hS1=FunhS1(S1,Flag1,Flag2)
%% 构造观测值的估计值向量的子函数
%提取估计的移动台坐标
x=S1(1);
y=S1(2);
%三个基站的横纵坐标
x1=0;
y1=0;
x2=5;
y2=8.66;
x3=10;
y3=0;
%计算移动台到三个基站的距离（估计值）
d1=((x-x1)^2+(y-y1)^2)^0.5;
d2=((x-x2)^2+(y-y2)^2)^0.5;
d3=((x-x3)^2+(y-y3)^2)^0.5;
M=2+sum(Flag2);
hS1=zeros(M,1);
if Flag1==1%以第一个基站为基准计算TDOA数据
hS1(1)=d2-d1;
hS1(2)=d3-d1;
elseif Flag1==2%以第二个基站为基准计算TDOA数据
hS1(1)=d1-d2;
hS1(2)=d3-d2;
elseif Flag1==3%以第三个基站为基准计算TDOA数据
hS1(1)=d1-d3;
hS1(2)=d2-d3;
else
error('Flag1格式不正确，它只能取1,2,3');
end
h1=atan2(y-y1,x-x1);
h2=atan2(y-y2,x-x2);
h3=atan2(y-y3,x-x3);
if Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==0
%空语句
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==0
hS1(3)=h1;
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==0
hS1(3)=h2;
elseif Flag2(1)==0&&Flag2(2)==0&&Flag2(3)==1
hS1(3)=h3;
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==0
hS1(3:4)=[h1;h2];
elseif Flag2(1)==1&&Flag2(2)==0&&Flag2(3)==1
hS1(3:4)=[h1;h3];
elseif Flag2(1)==0&&Flag2(2)==1&&Flag2(3)==1
hS1(3:4)=[h2;h3];
elseif Flag2(1)==1&&Flag2(2)==1&&Flag2(3)==1
hS1(3:5)=[h1;h2;h3];
else
error('Flag2格式不正确，它的元素只能取0或者1');
end
