基于PLE结合卡尔曼滤波的RSSI定位算法matlab仿真

1.算法运行效果图预览

2.算法运行软件版本

MATLAB2022a

3.算法理论概述

         基于PLE（Power-Law Equalizer）结合卡尔曼滤波的RSSI（Received Signal Strength Indicator）定位算法是一种利用无线信号强度进行位置估计的方法。该方法通过PLE算法对RSSI进行预处理，然后使用卡尔曼滤波器对处理后的数据进行位置和速度的估计。其整体流程图如下图所示：

一、基本原理

      PLE算法：PLE算法是一种用于提取信号特征的方法，它可以削弱多径效应等干扰因素对RSSI的影响，提高位置估计的准确性。PLE算法的核心思想是对接收到的信号强度进行幂次变换，将非线性关系转化为线性关系。具体公式如下：

Y = X^α

       其中，X表示接收到的信号强度，Y表示经过PLE处理后的信号强度，α为PLE算法的参数，需要根据实际环境进行调整。

      卡尔曼滤波器：卡尔曼滤波器是一种高效的递归滤波器，它可以通过对过去和现在的测量结果进行加权，估计未来的状态变量。在RSSI定位中，卡尔曼滤波器可以用于估计被定位物体的位置和速度。具体公式如下：

预测步骤：

X_pred = FX_est + BU

P_pred = FP_estF^T + Q

更新步骤：

Z_pred = HX_pred

Y = Z - Z_pred

K = P_predH^T*(HP_predH^T + R)^(-1)

X_est = X_pred + KY

P_est = (I - KH)*P_pred

其中，X_est表示估计的状态变量（即位置和速度），P_est表示估计误差协方差矩阵，F表示状态转移矩阵，B表示控制输入矩阵，U表示控制输入变量，Z表示测量值，H表示观测矩阵，Q表示过程噪声协方差矩阵，R表示测量噪声协方差矩阵，K表示卡尔曼增益矩阵，Y表示测量残差，I表示单位矩阵。

二、算法流程

初始化：设定初始位置、速度、PLE算法的参数α、卡尔曼滤波器的参数（F、B、H、Q、R）等。

PLE处理：对接收到的RSSI进行PLE处理，得到处理后的信号强度。

卡尔曼滤波：将处理后的信号强度作为测量值Z，使用卡尔曼滤波器进行位置和速度的估计。

更新估计值：根据卡尔曼滤波器的输出结果，更新估计的位置和速度。

迭代处理：重复执行步骤2-4，直到达到设定的迭代次数或收敛条件。

输出结果：输出最终估计的位置和速度。

三、优缺点

基于PLE结合卡尔曼滤波的RSSI定位算法具有以下优点：

可以削弱多径效应等干扰因素对RSSI的影响，提高位置估计的准确性。

通过对过去和现在的测量结果进行加权，可以减小测量噪声对位置估计的影响。

可以有效地利用RSSI的时空相关性，提高位置估计的稳定性。

具有较好的鲁棒性和适应性，可以适用于不同的环境和应用场景。

4.部分核心程序

for Num_xb = Num_xb2
    Num_xb
    Indx = Indx + 1;
    Dis        = RoomLength/(Num_xb-1); 
    for m=1:Stimes
        m
        rng(m);
        %生成节点坐标
        Position_X = (0.7*rand)*RoomLength;
        Position_Y = (0.7*rand)*RoomWidth;
        Position   = [Position_X,Position_Y];
        %计算节点到信标的距离
        for i=1:Num_xb
             XB(:,i)   = [i;(i-1)*Dis;0];
             Dist(:,i) = sqrt((Position_X-((i-1)*Dis))^2+Position_Y^2);
        end
        %基于RSS的定位算法  
        for i=1:Num_xb
            Number_rssi(1,i) = i;
            %每个信标节点的RSSI值
            if Dist(i) > Good_radius  
               Number_rssi(2,i) = 0;
            else
               Number_rssi(2,i) = func_RSSI_cal(Dist(i),Alpha);
            end
        end
        Number_rssi2 = Number_rssi;
        
        Number_rssi_save{m} = Number_rssi;
        Position_X2{m}      = Position_X;
        Position_Y2{m}      = Position_Y;
        %进行卡尔曼滤波
        %进行卡尔曼滤波
        %进行卡尔曼滤波
        tmps            = Number_rssi_save{m}(2,:);
        kalman_dat2{m}  = func_kalman(tmps); 
 
        Number_rssi(1,:)= Number_rssi_save{m}(1,:);
        Number_rssi(2,:)= kalman_dat2{m};
        Position_X  = Position_X2{m};
        Position_Y  = Position_Y2{m};
        
        %将RSSI值从大到小排列  
        for i = 1:Num_xb
            for j = i:Num_xb
                if Number_rssi(2,i) < Number_rssi(2,j)
                   a = Number_rssi(1,j);
                   b = Number_rssi(2,j);
                   Number_rssi(2,j) = Number_rssi(2,i);
                   Number_rssi(1,j) = Number_rssi(1,i);
                   Number_rssi(1,i) = a;
                   Number_rssi(2,i) = b;
                end
            end
        end
        %RSSI值最大的信标的距离
        for i=1:Best_xb
            Dist(i) = Dis*( (func_RSSI_cal(Dis,Alpha)/Number_rssi(2,i))^(1/2.8) );
        end
        %求未知节点坐标
        for i=1:Best_xb
            Beaconn(1,i) = XB(2,Number_rssi(1,i));
            Beaconn(2,i) = XB(3,Number_rssi(1,i));
        end
        All_num=Best_xb; 
        for i=1:2
            for j=1:(All_num-1)
                a(i,j) = Beaconn(i,j)-Beaconn(i,All_num);
            end
        end
        A =-2*(a');
        for i=1:(All_num-1)
            B(i,1)=Dist(i)^2-Dist(All_num)^2-Beaconn(1,i)^2+Beaconn(1,All_num)^2-Beaconn(2,i)^2+Beaconn(2,All_num)^2;
        end
        %计算X坐标
        X1    = pinv(A'*A)*A'*B;
        X_pos = X1(1,1);
        %计算Y坐标
        z    = 0;
        for j=1:Best_xb    
            z = z + sqrt(abs(Dist(j)^2-(X_pos-Beaconn(1,j))^2));
        end
        Y_pos = z/Best_xb;
        Loc = [X_pos;Y_pos];
        %点位误差
        error1(m) = sqrt((abs(Position_X-Loc(1)))^2+(abs(Position_Y-Loc(2)))^2);
        %横坐标误差
        error2(m) = abs(Loc(1)-Position_X);
        %纵坐标误差
        error3(m) = abs(Loc(2)-Position_Y);
        Number_rssis(:,m) = Number_rssi(2,:); 
    end
    Number_rssixb{Indx} = mean(Number_rssis,2);
    Number_xb{Indx}     = [1:Num_xb];
end
figure;
semilogy(Number_xb{1},Number_rssixb{1},'b-o');
grid on;
xlabel('信标数目');
ylabel('RSSI');
legend('信标数:30,RSSI排序后仿真图');
save result.mat Number_xb Number_rssixb