• 大小: 6KB
    文件类型: .zip
    金币: 1
    下载: 0 次
    发布日期: 2021-05-28
  • 语言: Matlab
  • 标签: 定位算法  MATLAB  

资源简介

主要是基于卡尔曼滤波的基本入门的处理,为新手入门简单介绍下

资源截图

代码片段和文件信息

function [MXMYSS]=ExtendedKalmanFilter(D1D2D3A1A2A3Flag1FLAG2S0P0SigmaRSigmaAOA)
%% 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矩阵,取值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&&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(SigmaRSigmaAOAFlag2)
%% 产生R矩阵的子函数
m=sum(Flag2);
B=[-110;
   -101];
R11=B*[SigmaR00;0SigmaR0;00SigmaR]*(B‘);
R12=zeros(2m);
R21=zeros(m2);
if m==0
    R22=[];
elseif m==1
    R22=SigmaAOA;
elseif m==2
    R22=[SigmaAOA       0;
                0SigmaAOA];

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        5031  2018-03-27 15:45  基于卡尔曼滤波的定位算法MATLAB程序\ExtendedKalmanFilter.m
     文件        4743  2018-03-27 16:42  基于卡尔曼滤波的定位算法MATLAB程序\Untitled2.m
     文件         192  2018-03-27 16:37  基于卡尔曼滤波的定位算法MATLAB程序\Untitled4.m
     文件        2642  2018-03-30 20:22  基于卡尔曼滤波的定位算法MATLAB程序\test_ekf.m
     目录           0  2018-03-30 20:22  基于卡尔曼滤波的定位算法MATLAB程序\

评论

共有 条评论