室内定位——卡尔曼滤波原理、Matlab与javascript实现

  前言：在基于Beacon的室内定位中，由于获取的RSSI值中含有大量噪声，故对RSSI值进行去噪是非常有必要的。

  1.卡尔曼滤波（Kalman Filter, KF）简介

  卡尔曼滤波(Kalman Filter, KF)算法是1960年美国科学家卡尔曼提出的一种线性最小方差统计估算方法，卡尔曼滤波器适用于对时变信号的实时处理（RSSI值就是一种时变的信号）。Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。

  2.卡尔曼滤波原理

  作为一个卡尔曼滤波的小白，我在查阅关于卡尔曼滤波的相关资料时发现很多优秀且通熟易懂的文章，如果你想弄懂卡尔曼滤波的原理强烈推荐下面两篇文章，我自己就不赘述了。

  （1）. 【传感器融合】扩展卡尔曼滤波的逐步理解与实现（上）
  （2） 扩展卡尔曼滤波新手教程（一）----中文版

  如果你不想看复杂的原理推导，想直接上手的话，可以看下面两篇文章。

   （1）. 卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用这篇文章中包含有C语言代码。
   （2）. 卡尔曼滤波的简单实现(Matlab & OC)这篇文章中包含了Matlab代码。

  3.卡尔曼滤波（一维）Matlab实现

  我利用手机在距离id为AC:23:3F:20:D3:75的Beacon 1米处和2米处各采集了50组RSSI值，其中前50组是距离1米处，后50组是距离2米处。
  （1）首先在Matlab中创建名为kalman_filter_1mand2m_75_50.m的文件，代码如下：

% kalman_filter_1mand2m_75_50.m
% AC:23:3F:20:D3:75 在1m和2m处各获取50个rssi值
clear;
data = [-57,-55,-59,-58,-68,-56,-55,-59,-70,-68,-59,-55,-57,-68,-55,-58,-55,-68,-69,-56,-57,-69,-68,-55,-58,-67,-66,-55,-67,-54,-55,-58,-67,-68,-55,-57,-67,-54,-57,-65,-69,-58,-68,-66,-54,-55,-57,-58,-66,-65,-70,-73,-74,-94,-77,-76,-75,-72,-73,-69,-70,-83,-74,-84,-73,-78,-71,-83,-80,-72,-75,-74,-73,-71,-76,-74,-70,-75,-70,-76,-74,-72,-74,-76,-70,-76,-69,-82,-78,-76,-75,-70,-73,-72,-70,-71,-68,-74,-72,-68];
%调用kalman_filter函数对data中的RSSI值进行滤波
%我这里选择的Q为0.000001，R为0.0001，初始的最佳估计值x0为-61，P为1
%注：其中Q和R可以根据自己获取的RSSI值不断进行调整，通过不断试验找到RSSI值滤波后拟合最好的Q和R，而P的初始值则不能设置为0，x0和P的初值是可以随便设的，强大的卡尔曼滤波器马上就能抹除不合理之处。
result = kalman_filter(data,0.000001,0.0001,-61,1);
i = 1:length(data);
plot(i,data,'bo--',i,result,'r*-');
disp(result);
legend('信号测量值','卡尔曼滤波后','Location','northwest')
xlabel('采集序号')
ylabel('RSSI值/dBm')

  （2）其次在Matlab中创建名为kalman_filter.m的文件，即写上面的kalman_filter（）函数的代码，代码如下：

% kalman_filter.m
%获取传过来的ＲＳＳＩ值
function X = kalman_filter(data,Q,R,x0,P0)

N = length(data);
K = zeros(N,1);
X = zeros(N,1);
P = zeros(N,1);
 
X(1) = x0;
P(1) = P0;
 
for i = 2:N
　　　％ｋ为卡尔曼增益
    K(i) = P(i-1) / (P(i-1) + R);
    ％ｘ（ｉ）为ｉ时刻的最优估计值，data(i)为ｉ时刻的测量值
    X(i) = X(i-1) + K(i) * (data(i) - X(i-1));
    ％Ｐ（ｉ）为ｉ时刻的最优协方差误差
    P(i) = P(i-1) - K(i) * P(i-1) + Q;
end

  （3）最后在Matlab中的kalman_filter_1mand2m_75_50.m中执行，就得到如下效果，卡尔曼滤波后的RSSI值拟合效果较好：

  3.卡尔曼滤波（一维）javascript实现

  我在做基于Beacon的室内定位时是基于微信小程序的，故以下js代码是在微信小程序中的js中写的。在实际应用中，RSSI值是一个时变的信号，卡尔曼滤波是对时变信号进行处理，所以不可能像Matlab中代码一样先把所有的RSSI值获取放在数组中，再对其进行滤波。下面的代码涉及到三个js代码页面，index.js是专门获取RSSI值的（如果不会可以看我的这篇文章室内定位——如何在微信小程序中获取Beacon的RSSI值），position.js是用来进行室内定位的，还有App.js。
  (1)首先在App.js中设置全局变量：

//app.js
App({
  globalData: {
    //从index.js中获取的实时Beacon的rssi值
    BeaconInfo:[],
    //k-1时刻的系统协方差
    last_p:1,
    //k时刻系统估算协方差
    now_p:0,
    //卡尔曼增益
    k:0,
    //k时刻状态变量的最优值,即滤波后的值，初始值设置为-61
    out:-61,
    //过程噪声协方差
    Q:0.000001,
    //观测噪声协方差
    R:0.0001,
    }
    })

  (2)然后在positon.js中写一个卡尔曼滤波函数：

//卡尔曼滤波器
  KalmanFilter: function (rssi) {
        //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
        app.globalData.now_p = app.globalData.last_p + app.globalData.Q;
        //卡尔曼增益方程：卡尔曼增益=k时刻系统估算协方差/（k时刻系统估算协方差+观测噪声协方差）
        app.globalData.k = app.globalData.now_p / (app.globalData.now_p + app.globalData.R);
        //更新最优值方程：k时刻状态变量的最优值=状态变量的预测值+卡尔曼增益*（测量值-状态变量的预测值）
    app.globalData.out = app.globalData.out + app.globalData.k * (rssi - app.globalData.out);
        //更新协方差方程：本次的系统协方差付给lastp为下一次运算准备
        app.globalData.last_p = (1 - app.globalData.k) * app.globalData.now_p;
    return app.globalData.out;
  },

  (3)在positon.js的中写一个执行卡尔曼滤波函数：

//卡尔曼滤波执行函数
exKalmanfilter:function(){
     var that=this;
     //用来获取卡尔曼滤波前的RSSI值
     var RSSI=0;
     //卡尔曼滤波后的RSSI值
     var FilterRssi=0;
     //获取从index.js中传来的第一Beacon的RSSI值
     RSSI = app.globalData.BeaconInfo[0].rssis;
     //将RSSI传入 卡尔曼滤波函数中进行滤波
     FilterRssi = this.KalmanFilter(RSSI);
     console.log(FilterRssi);
},

  (4)最后在positon.js中的onLoad函数中执行并隔1秒中刷新一次，实时对RSSI进行卡尔曼滤波：

/**
   * 生命周期函数--监听页面加载
   */
  onLoad: function (options) {
  setInterval(this.exKalmanfilter, 1000);
  }，

  4.总结

  由于本人水平有限，故只做了一维的卡尔曼滤波的代码实现。如果上面中有什么错误或者您有什么更好的实现方法，欢迎您批评指正和评论交流。最后我也非常感谢上面我提到的四篇优秀的文章的作者，感谢他们的分享！