（一）多传感器无迹卡尔曼滤波（UKF）跟踪正弦波

目录

前言

一、基础知识

（一）无迹卡尔曼滤波

（二）简单凸组合融合

二、模型构建

（一）状态和观测模型构建

（二）单个滤波器仿真

（三）融合滤波

三、结果展示

总结

---

前言

        本项目介绍了一种跟踪正弦波的无迹卡尔曼（UKF）融合滤波算法。首先，介绍了项目用到的基础知识，然后结合实际应用进行了建模。在建立单个传感器的无迹卡尔曼滤波模型之后，通过简单凸组合的融合算法，将多个滤波器的状态估计值进行了融合。最后结合matlab程序进行了讲解，并展示了相应的仿真结果。

---

一、基础知识

（一）无迹卡尔曼滤波

        滤波算法的本质就是状态估计，是一种通过观测值对系统状态进行最优估计的算法。简单来讲就是：系统更精确的值 = 传感器观测值+系统预测值。

        最常用的滤波算法是卡尔曼滤波算法，但卡尔曼滤波算法仅能对线性高斯模型做出最优状态估计。实际应用中会存在很多非线性函数，比如平方函数、指数函数以及三角函数等，这就导致普通卡尔曼滤波算法已经不适用。本项目中的正弦函数就是非线性函数，需要使用非线性系统的滤波算法。通常，我们可以利用线性化方法将非线性问题转化为近似的线性问题进行滤波，比如拓展卡尔曼滤波（EKF）算法。但是无迹卡尔曼滤波（UKF）算法是在利用线性滤波框架的基础上，通过无迹变换（UT）来处理均值和协方差的非线性传递问题。

       无迹卡尔曼滤波（UKF）算法的核心就是利用无迹变换（UT）处理均值和协方差。 无迹变换（UT）是指在估计点附近进行确定性采样，用这些采样点的高斯密度近似状态的概率密度函数。实现方法：在原状态分布中按照一定规则选取一些采样点，使这些采样点的均值和协方差等于原始状态的均值和协方差；将这些点带入非线性函数汇总，得到相应的非线性函数值点集，通过这些点集求取变换后的均值和协方差，这样得到的非线性变换的均值和协方差最少有二阶精度。对于高斯分布来说，可以达到三阶精度，其采样点的选择是基于先验均值和先验协方差矩阵的平方根的相关列实现的。

（二）简单凸组合融合

        信息融合具有多种形式，通常分为集中式融合和分布式融合方式。此处运用分布式融合方式对多个传感器的信息进行融合。分布式结构中每个传感器都有自己的处理器,在进行预处理后将滤波结果传送到中心节点进行融合处理。其中比较实用的是简单凸组合融合方法，但是此方法仅适用于互协方差可以忽略不计的场景下。此方法实现简单，应用范围广；并且即使在估计误差相关的情况下也是准最佳的。

        假设存在n个传感器。每个传感器的状态估计值为，误差协方差矩阵为,则对应的状态估计值和系统误差为：

        系统误差： 

        状态估计值：

二、模型构建

（一）状态和观测模型构建

        首先，构建相位变化的状态模型。在本项目中，状态变化值为相位，程序中利用 “°” 作为相位的度量单位。对应的状态变量为二维变量，分别表示系统的相位值和相位变化速度，此处假设系统相位发生匀速变化，即相位均匀增加。

        状态转移方程为：

        状态转移矩阵：

        过程噪声：

        之后，利用正弦函数对相位进行正弦变化就得到时变的正弦波；附加上系统噪声之后就得到了系统的观测值。

        观测方程：

        非线性观测函数：

        观测噪声：

        此处假设有10个滤波器对目标状态进行滤波，则对应的模型程序为：

clc;
close all;
%%
B = 10;  %滤波器个数
M = 1;   %状态维数
A = 3;%正弦信号的幅度
N = 1000;%采样点数
x= 1:0.5:N;
[~,N] = size(x);
Z_handle_1 = zeros(B,N);
nf = 0.001;%噪声强度
Q = cell(1,N);
Q(:) = {zeros(2,2)};
for i=1:B
Q{i} = nf*diag([1,1]);
end
R = 0.5*ones(1,B);
F = [1 1;0 1];
for j = 1:B
    X_data = zeros(2,N);
    Z_data = zeros(1,N);
    for i = 1:N
        if i ==1
            X_data(:,i)  =  F*[0,0.5]';
        else
            X_data(:,i)  =  F*X_data(:,i-1);
        end
    end
    X_data = X_data + sqrt(Q{j})*randn(2,N);
    Z_handle_1(j,:) = A*sind(X_data(1,:))+sqrt(R(j))*randn(1,N);
end

（二）单个滤波器仿真

        建立系统模型之后，对单个滤波器进行滤波迭代。首先，设定仿真初值，因为采样间隔为0.5°，且相位变化速度也为0.5°。因此假设状态初值和协方差初值为：

                       

        对应的滤波程序为：

for i = 1:N
    if i ==1
    X_mid(:,1) = [0.5,0.5]';
    P_mid{1}= diag([1,1]);
    else
    [X_mid(:,i),P_mid{i}]= UKF(X_mid(:,i-1),Z(1,i),P_mid{i-1},R,F,Q); 
    end
end

（三）融合滤波

        在得到10个传感器的滤波值之后，根据简单凸组合的准则对多个传感器的状态估计值进行融合，对应代码为：

%信息融合
X_SF = zeros(M,N);
P_SF = zeros(M,N);
for i = 1:N
X_mid = X_all(:,i);
P_mid = P_all(:,i);
%融合
P_SF_inv = 0;
X_cal = 0;
for j = 1:B
    P_SF_inv = (eye(M)/P_mid(j)) + P_SF_inv ;
    X_cal = (eye(M)/P_mid(j))*X_mid(j) + X_cal;
end
P_SF(1,i) = eye(M)/P_SF_inv;
X_SF(1,i) =(eye(M)/P_SF_inv)*X_cal;
end

三、结果展示

        对原始数据进行绘图，可以得到10个滤波器的原始数据图形，对应代码为：

figure(1)
plot(Z_handle_1');
title('原始数据','Fontsize',12);

        

        将第5个传感器的观测值、滤波后的状态估计值以及融合状态估计值绘制在一张图中，可以直观的感受到滤波效果，对应的代码为：

figure(2)
plot(1:N,Z_handle_1(5,:));
hold on;
plot(1:N,X_all(5,:),'linewidth',1.5);
hold on;
plot(1:N,X_SF,'linewidth',1.5);
legend('观测值','单个滤波器UKF','融合UKF','Fontsize',12);

        

        将单个传感器的观测值误差、滤波后的状态估计值误差以及融合状态估计值误差绘制在一张图中，可以直观分析滤波误差，对应的代码为：

figure(3)
error_obser = Z_handle_1(5,:)-A*sind(x);
error_single = X_all(5,:)-A*sind(x);
error_fusion = X_SF-A*sind(x);
plot(1:N,error_obser,'linewidth',1.5);
hold on;
plot(1:N,error_single,'linewidth',1.5);
hold on;
plot(1:N,error_fusion,'linewidth',1.5);
legend('观测误差','单个滤波器UKF误差','融合UKF误差','Fontsize',12);

        通过仿真结果我们可以知道，无迹卡尔曼滤波（UKF）能对正弦波进行有效的跟踪。单个滤波的误差小于观测数据误差，证明滤波算法有效。同时融合后的误差小于单个滤波器的误差，证明融合算法有效。仿真结果表明，所提融合滤波算法能够对正弦波进行有效跟踪。

---

总结

        以上就是今天要讲的内容，本文介绍了一种用于跟踪正弦波的融合滤波算法，结果表明滤波算法能够对正弦波进行有效跟踪，并且相比于单个传感器滤波，多传感器融合能够有效提升滤波精度。