tdoa/aoa定位的扩展卡尔曼滤波定位算法matlab源码,03TDOAAOA定位的扩展卡尔曼滤波算法MATLAB源代码...

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

欢迎访问 GreenSim 团队主页→ 邮箱：greensim@第 1 页TDOA/AOA定位的扩展卡尔曼滤波算法 MATLAB源代码TDOA/AOA 是无线定位领域里使用得比较多的一种定位体制，扩展卡尔曼滤波器是最经典的非线性滤波算法，可用于目标的定位和动态轨迹跟踪。function [MX,MY,SS]=ExtendedKalmanFilter(D1,D2,D3,A1,A2,A3,Flag1,FLAG2,S0,P0,SigmaR,SigmaAOA)%% TDOA/AOA 定位的扩展卡尔曼滤波定位算法% GreenSim 团队——专业级算法设计TDOA2=D3-D1;elseif Flag1==2TDOA1=D1-D2;TDOA2=D3-D2;elseif Flag1==3TDOA1=D1-D3;TDOA2=D2-D3;elseerror( Flag1 输入有误，它只能取 1,2,3 );欢迎访问 GreenSim 团队主页→ 邮箱：greensim@第 2 页end%% 第二步：构造两个固定的矩阵%构造状态转移矩阵 ΦPhi=[1, 0,0.025, 0;0, 1, 0,0.025;0, 0, 1, 0;0, 0, 0, 1];%构造 W 的协方差矩阵 QSigmaU=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:NFlag2=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 和 P0P2=P1-K*H*P1;S0=S2;P0=P2;%记录滤波输出值MX(i)=S2(1);MY(i)=S2(2);SS(:,i)=S2;endfunction Z=FunZ(TDOA1,TDOA2,A1,A2,A3,SigmaR,SigmaAOA,Flag2,i)%调用构造观察向量的子函数欢迎访问 GreenSim 团队主页→ 邮箱：greensim@第 3 页m=sum(Flag2);Z=zeros(2+m,1);Z(1)=TDOA1(i);Z(2)=TDOA2(i);if Flag2(1)==0elseif Flag2(1)==0elseif Flag2(1)==0elseif Flag2(1)==1Z(4)=A2(i);elseif Flag2(1)==1Z(4)=A3(i);elseif Flag2(1)==0Z(4)=A3(i);elseif Flag2(1)==1Z(4)=A2(i);Z(5)=A3(i);elseerror( Flag2 格式不正确，它的元素只能取 0 或者 1 );endfunction 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==0R22=[];elseif m==1R22=SigmaAOA;elseif m==2R22=[SigmaAOA, 0;0,SigmaAOA];elseif m==3欢迎访问 GreenSim 团队主页→ 邮箱：greensim@第 4 页R22=[SigmaAOA, 0, 0;0,SigmaAOA, 0;0, 0,SigmaAOA];elseerror( Flag2 格式不正确，它的元素只能取 0 或者 1 );endR=[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;elseerror( Flag1 格式不正确，它只能取 1,2,3 );endh1=atan2(y-y1,x-x1);h2=atan2(y-y2,x-x2);h3=atan2(y-y3,x-x3);if Flag2(1)==0elseif Flag2(1)==0elseif Flag2(1)==0elseif Flag2(1)==1h2];elseif Flag2(1)==1h3];elseif Flag2(1)==0h3];elseif Flag2(1)==1h2;h3];elseerror( Flag2 格式不正确，它的元素只能取 0 或者 1 );end