蓝牙定位-高斯滤波、卡尔曼滤波、滑动平均滤波（ matlab）

自己在做毕设的时候，发现网上很少有关于蓝牙定位过程中对rssi滤波处理的matlab代码，要不就是收费（誓死不花一分钱）要不就是对于图像处理的一系列滤波算法，所以小陈把自己写的滤波算法分享出来，希望能够对做同样定位的研友们有所帮助！

滑动平均滤波（详细算法自行搜索哦）

function res = MovingAverage(input,N)
%% input为平滑前序列(列向量和行向量均可)；N为平滑点数（奇数）；res返回平滑后的序列(默认行向量)。
sz = max(size(input));
n = (N-1)/2;
res = [];
for i = 1:length(input)
    if i <= n
        res(i) = sum(input(1:2*i-1))/(2*i-1);
    elseif i < length(input)-n+1
        res(i) = sum(input(i-n:i+n))/(2*n+1);
    else
        temp = length(input)-i+1;
        res(i) = sum(input(end-(2*temp-1)+1:end))/(2*temp-1);
    end
end
end

卡尔曼滤波

% 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);

    X(i) = X(i-1) + K(i) * (data(i) - X(i-1));
    P(i) = P(i-1) - K(i) * P(i-1) + Q;
end

高斯滤波

% 功能：对一维信号的高斯滤波，头尾r/2的信号不进行滤波
% r     :高斯模板的大小推荐奇数
% sigma :标准差
% y     :需要进行高斯滤波的序列
function y_filted = Gaussian_filter(r, sigma, y)

% 生成一维高斯滤波模板
GaussTemp = ones(1,r*2-1);
for i=1 : r*2-1
    GaussTemp(i) = exp(-(i-r)^2/(2*sigma^2))/(sigma*sqrt(2*pi));
end

% 高斯滤波
y_filted = y;
for i = r : length(y)-r+1
    y_filted(i) = y(i-r+1 : i+r-1)*GaussTemp';
end

画图代码

clear;
input_y = [-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
];
%设置滑动平均
arr_MA = MovingAverage(input_y, 17);

%卡尔曼滤波
result = kalman_filter(input_y,0.000001,0.0001,-61,1);

% 设置高斯模板大小和标准差
r        = 3;
sigma    = 1;
y_filted = Gaussian_filter(r, sigma, input_y);

% 作图对比
%axis([0,50,-95,-60])

m = 1:length(input_y);

plot(m,input_y,'r-',m,arr_MA ,'b-',m,result,'m-',m,y_filted,'k-' );
axis([0,50,-95,-60])
title('滤波对比');
legend('原始数据','滑动平均滤波法','卡尔曼滤波','高斯滤波','Location','northwest')
xlabel('采集序号')
ylabel('RSSI值/dBm')