基于卡尔曼滤波的定位算法MATLAB程序

function [MX,MY,SS]=ExtendedKalmanFilter(D1,D2,D3,A1,A2,A3,Flag1,FLAG2,S0,P0,SigmaR,SigmaAOA) %% TDOA/AOA定位的扩展卡尔曼滤波定位算法 %% 此程序为GreenSim团队原创作品，转载请注明 %% 欢迎访问GreenSim团队的主页http://blog.sina.com.cn/greensim %% 欲与程序原作者进行技术交流，请发邮件Email:aihuacheng@gmail.com %% 输入参数列表 % 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