使用卡尔曼滤波方法滤除UWB传感器二维坐标噪点

针对室内环境下一些移动机器人，通过UWB室内定位传感器采集其运动中的X和Y坐标数据，通过拟合发现与实际路径存在较大误差。采用卡尔曼滤波法对其进行一定的滤波，得到较为平滑的数据，对于本人之后工作处理效果足够，但可能对精确化定位依然不够。

滤波前后路径对比：

贴一段通过Matlab实现卡尔曼滤波的代码:

N=size(ZM,1);               %采样点数.
ZM=ZM';
Q=10^(-6)*eye(4);     %Q是输入(状态,过程）白噪声w的方差矩阵
W=sqrt(Q)*randn(4,N);
R=10^(-2)*eye(2);                %观测噪声方差
V=sqrt(R)*randn(2,N);


x=zeros(4,N);
U=zeros(2,N);    %控制变量,加速度


dt=1;                %采样时间间隔
A=[1 0 dt 0; 0 1 0 dt;0 0 1 0;0 0 0 1];       %状态转移矩阵
H=[1 0 0 0;0 1 0 0];                %观测矩阵
x(1,1)=ZM(1,1);x(2,1)=ZM(2,1);
x_pre=zeros(4,N);
xkf=zeros(4,N);
xkf(:,1)=x(:,1);       %状态估计初始值。
P0=diag([1,1,0.1,0.1]); %误差协方差矩阵初始值。

可叠加使用多种预处理及最小方差等方法和卡尔曼滤波融合进行数据处理，可得到较好的处理效果以及精确化数据。