03TDOA_AOA定位的扩展卡尔曼滤波算法MATLAB源代码

1、欢迎访问 GreenSim 团队主页宀 邮箱：greensimTD0A/A0A 定位的扩展卡尔曼滤波算法 MATLAB源代码TD0A/A0A是无线定位领域里使用得比较多的一种定位体制，扩展卡尔曼 滤波器是最经典的非线性滤波算法，可用于目标的定位和动态轨迹跟踪。fun ctio nMX,MY ,SS=ExtendedKalmanFilter(D1,D2,D3,A1,A2,A3,Flag1,FLAG2,S0,P0,SigmaR,SigmaA OA)% TDOA/AOA定位的扩展卡尔曼滤波定位算法% GreenSim团队一一专业级算法设计&代写程序% 欢迎访问 GreenSim 团队主页宀

2、%输入参数列表%D1基站1和移动台之间的距离%D2基站2和移动台之间的距离%D3基站3和移动台之间的距离%A1基站1测得的角度值%A2基站2测得的角度值%A3基站3测得的角度值% Falg1 1X 1矩阵，取值1,2,3,表明是以哪一个基站作为基准站计算TDOA数据的% FLAG2 N X 3矩阵，取值0和1，每一行表示该时刻各基站的AOA是否可选择，%1表示选择AOA数据，FLAG2并非人为给定，而是由LOS/NLOS检测模块确定% S0初始状态向量，4 X 1矩阵% P0预测误差矩阵的初始值，4X 4的矩阵% SigmaR 无偏/有偏卡尔曼输出距离值的方差，由事先统计得到% SigmaAO

3、A 选择AOA数据的方差，生成 AOA数据的规律已知，因此可以确定%输出参数列表% MX% MY%第一步：计算TDOA数据if Flag 仁=1TDOA1=D2-D1;TDOA2=D3-D1;elseif Flag 1=2TDOA1=D1-D2;TDOA2=D3-D2;elseif Flag 1=3TDOA1=D1-D3;TDOA2=D2-D3;elseerror('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的协方差矩阵

4、QSigmaU=0.00001;%噪声方差取很小的值Q=0,0, 0, 0;0,0, 0, 0;0,0,SigmaU,0;0,0,0,SigmaU;%第三步：输出数据初始化N=le ngth(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:NFlag2=FLAG2(i,:);%当前各信道环境的LOS/NLOS判据R=FunR(SigmaR,SigmaA0A,Flag2);% 调用产生 R 矩阵的子函数S1=Phi*S0;%由状态方程得到的预测

5、值H=FunH(S1,Flag1,Flag2);%调用产生、H矩阵的子函数P仁Phi*PO*(Phi')+Q;%计算上述预测值的协方差矩阵K=P1*(H')*i nv(H*P1*(H')+R);%计算滤波增益(加权系数)Z=Fu nZ(TDOA1,TDOA2,A1,A2,A3,SigmaR,SigmaAOA,Flag2,i);%调用构造观察向量的子函数hS仁FunhS1(S1,Flag1,Flag2);%调用构造观测值的估计值向量的子函数S2=S1+K*(Z-hS1);%加权得到滤波输出值%更新S0和P0P2=P1-K*H*P1;S0=S2;P0=P2;%记录滤波输出值

6、MX(i)=S2(1);MY(i)=S2(2);SS(:,i)=S2;endfunction Z=FunZ(TDOA1,TDOA2,A1,A2,A3,SigmaR,SigmaAOA,Flag2,i)%调用构造观察向量的子函数m=sum(Flag2);Z=zeros(2+m,1);Z(1)=TD0A1(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);elsei

7、f 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)=0Z(3)=A1(i);Z(4)=A2(i);elseif Flag2(1)=1 &&Flag2(2)=0&&Flag2(3)=1Z(3)=A1(i);Z(4)=A3(i);elsei

8、f Flag2(1)=0&&Flag2(2)=1 &&Flag2(3)=1Z(3)=A2(i);Z(4)=A3(i);elseif Flag2(1)=1 &&Flag2(2)=1 &&Flag2(3)=1Z(3)=A1(i);Z(4)=A2(i);Z(5)=A3(i);0或者1');elseerror('Flag2格式不正确，它的元素只能取endfunction R=FunR(SigmaR,SigmaAOA,Flag2)%产生R矩阵的子函数m=sum(Flag2);B=-1,1,0;-1,0,1;R1仁 B*Sig

9、maR,0,0;0,SigmaR,0;0,0,SigmaR*(B');R12=zeros(2,m);R21= zeros(m,2);if m=0R22=;elseif m=1R22=SigmaAOA;elseif m=2R22=SigmaAOA,0;0,SigmaAOA;elseif m=3R22=SigmaAOA,0,0;0,SigmaAOA,0;0,0,SigmaAOA;elseerror('Flag2格式不正确，它的元素只能取0或者1');endR=R11,R12;R21,R22;function hS1=FunhS1(S1,Flag1,Flag2)%构造观测值的

10、估计值向量的子函数%提取估计的移动台坐标x=S1(1);y=S1 (2);%三个基站的横纵坐标x1=0;y1=0;x2=5;y2=8.66;x3=10;y3=0;%计算移动台到三个基站的距离(估计值)d1=(x-x1)A2+(y-y1)A2)A0.5;d2=(x-x2)A2+(y-y2)A2)A0.5;d3=(x-x3)A2+(y-y3)A2)A0.5;M=2+sum(Flag2);hS1= zeros(M,1);if Flag仁=1%以第一个基站为基准计算TDOA数据hS1(1)=d2-d1;hS1( 2)=d3-d1;elseif Flag仁=2%以第二个基站为基准计算TDOA数据hS1(

11、1)=d1-d2;hS1( 2)=d3-d2;elseif Flag仁=3%以第三个基站为基准计算TDOA数据hS1(1)=d1-d3;hS1(2)=d2-d3;elseerror('Flag1格式不正确，它只能取1,2,3');endh1=ata n2(y-y1,x-x1);h2=ata n2(y-y2,x-x2);h3=ata n2(y-y3,x-x3);if Flag2(1)=0&&Flag2(2)=0&&Flag2(3)=0%空语句elseif Flag2(1)=1 &&Flag2(2)=0&&Flag2(

12、3)=0hS1 (3)=h1;elseif Flag2(1)=0&&Flag2(2)=1 &&Flag2(3)=0hS1 (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