有偏卡尔曼滤波器

1、有偏卡尔曼滤波器可以用来消除无线定位中的非视距误差，首次使用有偏卡尔曼滤波器的参考文献是下面两篇论文1刘琚，李静.一种在非视距环境中的TDOA/AOA混合定位方法J.通信学报，2005，26(5:63-67.2Bao Long Le, K. Ahmed, H. Tsuji. Mobile location estimator with NLOS mitigation using Kalman filteringC. IEEE Wireless Communications and Networking, 2003, 3:1969-1973.本源码为GreenSim团队原创，转载请注明。func

2、tion X,Flag=BiasedKalmanFilter(x,X0,P0,Phi,Gamma,Q,H,R,SIGMAD,SIGMA_NLOS% 用于校正NLOS误差的有偏卡尔曼滤波器% GreenSim团队原创作品，转载请注明% 本程序加入了非视距检测模块，对于NLOS使用有偏卡尔曼滤波，对于LOS使用标准卡尔曼滤波% 输入参数列表% x 输入距离观测值序列，1×N的行向量% X0 预测序列的初始值，1×2的列向量，包括距离和径向速度两个分量% P0 预测误差矩阵的初始值，2×2的矩阵% Phi 状态转移矩阵,2×2的矩阵% Gamma 噪声输入矩阵

3、，2×1的矩阵% Q 输入噪声协方差矩阵，1×1的矩阵% H 观测矩阵，1×2的向量% R 观测噪声协方差矩阵，1×1的矩阵% 输出参数列表% X 预测输出值% Flag NLOS标记，为1表示LOS，为0表示非视距%N=length(x;X=zeros(1,N;X(1=X0(1;Flag=ones(N,1;for i=2:Nif i>15%因为检测区间为15个样本YB=x(i-15:(i;%待检测的样本%YB=x(i-15:(i-X(i-15:(i;改成这个以后，无论怎么调整参数都不收敛YBstd=std(YB;elseYBstd=0;endif

4、 YBstd>3*sqrt(SIGMAD%如果局部样本标准差大于三倍（可改）观测误差的标准差，则判断为NLOSFlag(i=0;%设置调整参数lambda=1.4;%由状态方程得到的预测值X1=Phi*X0;%计算上述预测的协方差矩阵P1=Phi*P0*(Phi'+Gamma*Q*(Gamma'%计算滤波增益（加权系数）K=P1*(H'*inv(Q*P1*(H'+lambda*R;%计算观察值Y=x(i+lambda*R;%加权得到滤波输出值X2=X1+K*(Y-S*X1-SIGMA_NLOS;else%由状态方程得到的预测值X1=Phi*X0;%计算上述

5、预测的协方差矩阵P1=Phi*P0*(Phi'+Gamma*Q*(Gamma'%计算滤波增益（加权系数）K=P1*(H'*inv(P*P1*(H'+R;%计算观察值Y=x(i+R;%加权得到滤波输出值X2=X1+K*(Y-H*X1;end%记录和更新P2=(1,0;0,1-K*H*P1;X(i=X2(1;X0=X2;P0=P2;end 卡尔曼滤波仿真程序用于匀速运动的目标跟踪% kalman filteringload initial_track s; % y:initial data,s:data with noiseT=0.1;% yp denotes th

6、e sample value of position% yv denotes the sample value of velocity% Y=yp(n;yv(n;% error deviation caused by the random acceleration % known dataY=zeros(2,200;Y0=0;1;Y(:,1=Y0;A=1 T0 1; B=1/2*(T2 T'H=1 0;C0=0 00 1;C=C0 zeros(2,2*199;Q=(0.252; R=(0.252; % kalman algorithm ierationfor n=1:200i=(n-1

7、*2+1;K=C(:,i:i+1*H'*inv(H*C(:,i:i+1*H'+R;Y(:,n=Y(:,n+K*(s(:,n-H*Y(:,n;Y(:,n+1=A*Y(:,n;C(:,i:i+1=(eye(2,2-K*H*C(:,i:i+1;C(:,i+2:i+3=A*C(:,i:i+1*A'+B*Q*B'end% the diagram of position after filteringt=0:0.1:20;figure(2;yp=Y(1,:;plot(t,yp,'r.-'axis(0 20 0 20;xlabel('time'

8、ylabel('yp position'title('the track after kalman filtering'hold on;% the diagram of velocity after filteringfigure(3;yv=Y(2,:;plot(t,yv,'k.-'xlabel('time'ylabel('yv velocity'title('the velocity caused by random acceleration' 很标准的卡尔曼滤波t=0.1:0.1:6;x=exp

9、(t;i=1:60;plot(i,x,'r'hold on%以上是要预测的曲线（或者说是要跟踪的曲线）：y=exp(x;%系统方程：x(k+1=fi*x(k+gm*w(k%观测方程：z(k=h*x(k+v(kfi=1.1052;h=1;gm=1;w=randn(1,60;v=randn(1,60;xy(1=0;p(1=0;z(1=x(1+w(1;R=(std(v.2;Q=(std(w.2;k(1=fi*p(1*h'*inv(h*p(1*h'+R;pp(1=fi*p(1*fi'+gm*Q*gm'for i=2:60xy(i=fi*xy(i-1+k(i-1*(z(i-1-xy(i-1;k(i=pp(i-1*h'*inv(h*pp(i-1*h'+R;pp(i=fi*p(i-1*fi'+gm*Q*gm'p(i=pp(i-