（六）基于强跟踪滤波和Sage-Husa滤波的结合算法

追风者...

已于 2024-01-04 22:09:35 修改

阅读量3k

点赞数 26

分类专栏：滤波算法仿真与实践文章标签：算法

于 2024-01-04 22:08:49 首次发布

本文链接：https://blog.csdn.net/h320130/article/details/135008267

版权

滤波算法仿真与实践专栏收录该内容

13 篇文章

订阅专栏

本文介绍了一种基于强跟踪滤波和Sage-Husa滤波的结合算法，通过动态选择滤波方式，有效降低噪声影响并实现目标的精准跟踪。通过CV模型的仿真验证，展示了算法的实际效果和噪声抑制能力。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

前言

本文介绍了一种基于强跟踪滤波和Sage-Husa滤波的结合算法，可以根据新息的大小对目标进行滤波算法的选择，进而实现目标的有效跟踪，首先，介绍了两种算法的基础知识以及结合算法的结构。最后，结合CV（匀速运动）模型进行了仿真。仿真结果表明，所提算法能够实现目标位置的精准滤波，有效降低噪声影响。

一、基础知识

（一）强跟踪滤波

强跟踪滤波器算法在卡尔曼滤波的基础上，引入了渐消因，通过卡尔曼增益使残差系列正交。

假设以下系统：

$\begin{cases} & x(k)=Fx(k-1)+w_{k} \\ & z_{k}=Hx(k)+v_{k} \end{cases}$

其中，F为过程转移矩阵； $w_{k}$ 和 $v_{k}$ 分别为过程噪声以及观测噪声，对应的噪声矩阵为 $Q_{k}$ 和 $R_{k}$ 。其具体计算过程如下图所示：

对应的代码程序如下（部分）：

%强跟踪滤波器STF代码
    n_state = 4;%状态维数为1                     
    rho=0.95;% 残差方差阵的遗忘因子初始值 (0.95<rho<0.995)
    beta=100; % 量测噪声的衰减因子选定值 (beta>=1)
    x = F*x; %一步预测
    r0=z-H*x;
    V0=(rho*V0+r0*r0')/(1+rho);%不用初始化，因为外部循环已经进行了判断
    N=V0-beta*R-H*Q*H';
    M=H*F*P*F'*H';
    eta=trace(N)/trace(M);
    if eta>1
        lamda=eta;
    else
        lamda=1;
    end

（二）Siga-Husa自适应滤波

常规卡尔曼滤波情况下，系统噪声和量测噪声为固定不变的经验值，滤波的状态估计会受到影响。Siga-Husa自适应滤波可以对噪声实现实时的和更新，能够减少系统对于经验值的依赖，提升滤波精度，减少滤波收敛的时间。适用于系统噪声和量测噪声均不是零均值，且噪声的均值和方差均未知的情况。

系统状态方程和观测方程设置如下：

$\begin{cases} & X_{k}=AX_{k-1}+W_{k-1} \\ & Z_{k}=H_{k}X_{k}+V_{k} \end{cases}$

其中， $A$ 为状态转移矩阵； $W_{k-1}$ 为系统噪声； $Z_{k}$ 为观测向向量； $H_{k}$ 为观测矩阵； $V_{k}$ 为观测噪声。

Siga-Hua自适应滤波主要的作用是估计过程噪声矩阵 $Q_{k}$ 和观测噪声矩阵 $R_{k}$ .其中 $W_{k}$ 和 $V_{k}$ 满足以下条件：

$\begin{cases} &E[W_{k}]=q_{k},E[W_{k},W_{j}^{T}]=Q_{k}\delta_{kj} \\ &E[V_{k}]=r_{k},E[V_{k},V_{j}^{T}]=R_{k}\delta_{kj} \\ & E[W_{k},V_{j}^{T}]= 0 \end{cases}$

对应的滤波过程如下：

$X_{k\mid k-1}=AX_{ k-1}+q_{k-1}$

$P_{k\mid k-1}=AP_{k-1}A^{T}+Q_{k-1}$

$K_{k}=P_{k\mid k-1}H^{T}\left [ HP_{k\mid k-1}H^{T} +R_{k}\right ]^{-1}$

$e_{k}=Z_{k}-HX_{k\mid k-1}-r_{k-1}$

$X_{k}=X_{k\mid k-1}+K_{k}e_{k}$

$P_{k} = [I-K_{k}H]P_{k/k-1}$

具有时变性能的噪声估计器：

$q_{k}=(1-d_{k})q_{k-1}+d_{k}(X_{k}-AX_{k-1})$

$Q_{k}=(1-d_{k})Q_{k-1}+d_{k}(K_{k}e_{k}e_{k}^{T}K_{k}^{T}+P_{k}-AP_{k-1}A^{T})$

$r_{k}=(1-d_{k})r_{k-1}+d_{k}(Z_{k}-HX_{k\mid k-1})$

$R_{k}=(1-d_{k})R_{k-1}+d_{k}(e_{k}e_{k}^{T}-HP_{k\mid k-1}H^{T})$

其中， $d_{k}=(1-b)/(1-b^{k+1})$ ； $b$ 为遗忘因子，实际取值为 $[0.95, 0.99]$ 。

对应部分仿真代码如下：

%滤波过程
X_est=F*X_input+q_input;                  %计算一步预测估计：X(k/k-1)
P_pre=F*P_input*F'+Q_input;               %一步预测估计的均方误差P(k/k-1)
e=Z_input-H*X_est-r_input;           %未完成卡尔曼滤波，此处应该为新息。即观测值减去预测观测值
K=P_pre*H'*inv((H*P_pre*H')+R_input);    %k时刻的增益阵
X_output=X_est+K*e;           %k时刻的状态估计X(k)
E_ek = (H*P_pre*H')+R_input;            %新息的期望,用作滤波的一部分，用来判断是否进行强跟踪滤波
P_output = (eye(M)-K*H)*P_pre;  %均方误差矩阵P(k)，P = (eye(M)-K*H)*P_pre*(eye(M)-K*H)'+K*R*K';
%系数更新
r_output = (1-d)*r_input +d*(Z_input-H*X_est);

（三）结合滤波算法

结合算法中，当残差较小时，采用Sage-husa滤波；残差过大时，采用强跟踪滤波，保持滤波收敛，防止滤波发散。对应的滤波过程示意图如下：

对应部分仿真代码如下：

    if ZZ <= W*trace(EE)%首先判断用哪种滤波方式；其次判断上一次用的滤波方式，决定是否进行初始化或者继承上一次的滤波结果
        if v{1,k-1}~=0%如果上次为Sage_Husa滤波则继承上次参数，如果不是就进行新的初始化
            [v(:,k),X(:,k),P{k}]=Sage_HusaKF(F_CV,H,v(:,k-1),k,X(:,k-1),P{k-1},Z(:,k));
        else
            [v(:,k),X(:,k),P{k}]=Sage_HusaKF(F_CV,H,v(:,1),k,X(:,k-1),P{k-1},Z(:,k));
        end
    else
        if V0{k-1}(1,1)~=0%如果上次为强滤波则继承上次参数，如果不是就进行新的初始化
            [X(:,k),P{k},V0{k}]=kalman_stf(F_CV,H,X(:,k-1),P{k-1},Z(:,k),Q,R,V0{k-1});
        else
            [X(:,k),P{k},V0{k}]=kalman_stf(F_CV,H,X(:,k-1),P{k-1},Z(:,k),Q,R,V0{1});
        end

    end

二、仿真验证

（一）模型构建

CV（匀速运动）模型的详细定义可以参考以下博客：

交互式多模型（IMM）中二维平面CV/CA/CT模型构建https://blog.csdn.net/h320130/article/details/135141783?spm=1001.2014.3001.5501

CV模型的参数设定如下：

采样时间：T = 1

传感器个数：B = 1

仿真点数：N = 300

初始向量：[40,2,60,-5]'

对应部分仿真代码如下：

%数据预定义
B = 1;  %传感器个数
M = 4;   %状态维数
T = 1;   %采样时间
N = 300;%仿真点数

%过程噪声矩阵
Q_initial1 = zeros(M,M);
Q_mid1 = [T^4/3, T^3/2;
    T^3/2,  T^2];%模型过程噪声协方差矩阵
Q_initial1(1:2,1:2) = Q_mid1; Q_initial1(3:4,3:4) = Q_mid1;
Q1 = sqrtm(Q_initial1);

%量测噪声矩阵
H=[1 0 0 0; 0 0 1 0];
nz = 2;%观测维数
R = cell(1,B);
R(:) = {1000*ones(nz,nz)};


%过程转移矩阵
F_CV = zeros(M,M);
F1 = [1 T ;
    0 1 ];
F_CV(1:2,1:2) = F1;F_CV(3:4,3:4) = F1;


%定义强度
nf = 0.2;
%定义空矩阵
X_real = zeros(M,N);
Z_obs = cell(1,B);
Z_obs(:) = {zeros(nz,N)};

for i = 1:N
    if i ==1
        X_real(:,i)  =  [40,2,60,-5]';
    else
        X_real(:,i)  =  F_CV*X_real(:,i-1)+nf*sqrt(Q1)*randn(M,1);
    end
end

for i = 1:B  
    for j = 1:N
    Z_obs{i}(:,j) = H*X_real(:,j) +sqrt(R{i})*randn(nz,1);
    end
end

（二）仿真结果

绘图部分代码如下：

%绘图部分
figure(1)
plot(Z_obs{1}(1,:),Z_obs{1}(2,:));
hold on ;
plot(X_all{1}(1,:),X_all{1}(3,:));
legend("观测轨迹","滤波后轨迹",'Fontsize',12)


error = sqrt((X_all{1}(1,:) - X_real(1,:)).^2 + (X_all{1}(3,:) - X_real(3,:)).^2);
error1 = sqrt((Z_obs{1}(1,:) - X_real(1,:)).^2 + (Z_obs{1}(2,:) - X_real(3,:)).^2);
figure(2)
plot(error1);
hold on;
plot(error);

legend("观测误差","滤波后误差",'Fontsize',12);
ylabel("RMSE",'Fontsize',12);
xlabel("时刻",'Fontsize',12)