[RFEDA原创]有偏卡尔曼滤波器Matlab源码

题目：有偏卡尔曼滤波器Matlab源码 vz6No%8X  
有偏卡尔曼滤波器可以用来消除无线定位中的非视距误差，首次使用有偏卡尔曼滤波器的参考文献是下面两篇论文 I
R JN  
[1]刘琚，李静.一种在非视距环境中的TDOA/AOA混合定位方法[J].通信学报，2005，26(5):63-67. ,+2!&"zD  
[2]Bao Long Le, K. Ahmed, H. Tsuji. Mobile location estimator with NLOS mitigation using Kalman filtering[C]. IEEE Wireless Communications and Networking, 2003, 3:1969-1973. ^Gi9&fS,  
本源码为GreenSim团队原创，转载请注明。 ;>hRj!  
function [X,Flag]=BiasedKalmanFilter(x,X0,P0,Phi,Gamma,Q,H,R,SIGMAD,SIGMA_NLOS) ;QO3^P}  
%% 用于校正NLOS误差的有偏卡尔曼滤波器 Z5/*iun  
% GreenSim团队原创作品，转载请注明 I;1W6uD=  
% 更多原创作品，请联系GreenSim团队QQ：761222791 Ig&H0S  
%% 本程序加入了非视距检测模块，对于NLOS使用有偏卡尔曼滤波，对于LOS使用标准卡尔曼滤波 {=K);z  
%% 输入参数列表 7[=\bL  
%  x     输入距离观测值序列，1×N的行向量 +@],JlYf  
%  X0    预测序列的初始值，1×2的列向量，包括距离和径向速度两个分量 &3gC&b^i  
%  P0    预测误差矩阵的初始值，2×2的矩阵 @}; vl  
%  Phi   状态转移矩阵,2×2的矩阵 43wm_4C!H  
%  Gamma 噪声输入矩阵，2×1的矩阵 ]31>0yj[Q  
%  Q     输入噪声协方差矩阵，1×1的矩阵
mR,w~wP  
%  H     观测矩阵，1×2的向量 )j,Y(V$P  
%  R     观测噪声协方差矩阵，1×1的矩阵 Fi+8|/5  
%% 输出参数列表 f/xQy}4+~E  
%  X     预测输出值 d*$L$1S  
%  Flag  NLOS标记，为1表示LOS，为0表示非视距 d?:KEi-<7  
%% T<? (KW  
N=length(x); FEY_(70  
X=zeros(1,N); ^_\S)P2c  
X(1)=X0(1); LV2#w_^I  
Flag=ones(N,1); %_Q+@9  
for i=2:N nA*Udrcn  
    if i>15%因为检测区间为15个样本 90wGS_P04  
        YB=x((i-15):(i));%待检测的样本 Nk63F&J7e  
        %YB=x((i-15):(i))-X((i-15):(i));改成这个以后，无论怎么调整参数都不收敛 6v"WI@b4  
        YBstd=std(YB); `D/<*e,#  
    else 8c`EB-y  
        YBstd=0; GFGW'}w-  
    end Es<& 6  
    if YBstd>3*sqrt(SIGMAD)%如果局部样本标准差大于三倍（可改）观测误差的标准差，则判断为NLOS TWSqn'<E  
        Flag(i)=0; @7HHi~1JK  
        %设置调整参数 Mi~x(W@}3  
        lambda=1.4; e5AZU7%.  
        %由状态方程得到的预测值 r4 $<,~  
        X1=Phi*X0; |N5r_V  
        %计算上述预测的协方差矩阵 h;Hg/jv  
        P1=Phi*P0*(Phi')+Gamma*Q*(Gamma'); B4@1WZn<8  
        %计算滤波增益（加权系数） RGgePeaw  
        K=P1*(H')*inv(Q*P1*(H')+lambda*R);
WOg pDs  
        %计算观察值 bG)EZ  
        Y=x(i)+lambda*R; Y`NwE  
        %加权得到滤波输出值 MJ"@  
        X2=X1+K*(Y-S*X1-SIGMA_NLOS); knn9s0'Q  
    else CdZ. T/x  
        %由状态方程得到的预测值 S>Z07d6&  
        X1=Phi*X0; 7Pe<0K)s(  
        %计算上述预测的协方差矩阵 Nl]_Ie6  
        P1=Phi*P0*(Phi')+Gamma*Q*(Gamma'); tm1#Lh0  
        %计算滤波增益（加权系数） 'XJqh|G  
        K=P1*(H')*inv(P*P1*(H')+R); [][ze2+b  
        %计算观察值 &|-jU+r}B  
        Y=x(i)+ .. *iX PG9XZ  
t|urvoz  

未注册仅能浏览部分内容，查看全部内容及附件请先 登录 或 注册