（二）多传感器拓展卡尔曼滤波（EKF）算法

滤波~

已于 2023-12-18 18:27:11 修改

阅读量1.8k

点赞数 28

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

于 2023-12-13 10:57:06 首次发布

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

版权

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

8 篇文章 13 订阅

订阅专栏

前言

本文介绍了一种用于多传感器的拓展卡尔曼滤波（EKF）算法。首先，介绍了EKF中运用到的一阶泰勒展开技术，介绍了EKF的本质，展示了滤波过程。之后，对于多传感器状态值估计中用到的简单凸组合技术进行了讲解。最后，结合一个实例和matlab程序对算法的具体实现过程进行了讲解。仿真结果证明了滤波融合算法的有效性和实用性。（可在资源中下载）

一、基础知识

（一）拓展卡尔曼滤波

常用的卡尔曼滤波算法仅能对线性高斯模型做出最优状态估计。实际应用中会存在很多非线性函数，比如平方函数、指数函数以及三角函数等，这就导致普通卡尔曼滤波算法已经不适用。通常，我们可以利用线性化方法将非线性问题转化为近似的线性问题进行滤波，比如拓展卡尔曼滤波（EKF）算法。

拓展卡尔曼滤波（EKF）算法的核心就是利用线性化将非线性滤波问题近似为线性滤波问题。对于一般的系统，一般是在估计值 $\widehat{x}_{k}$ 处将非线性函数 $f\left ( \ast \right )$ 和 $h\left ( \ast \right )$ 展开为一阶泰勒级数，忽略二阶以上项，进而得到近似的线性模型。之后利用卡尔曼滤波进行滤波处理。EKF的优点是计算渐变，但仅适用于滤波误差较小的场合。

非线性系统模型可以表示为：

$X(k+1) = f(X(k)) + w(k)$

$Z(k) = h(X(k)) + v(k)$

过程噪声： $w(k)\sim N(0,Q(k))$

观测噪声： $v(k)\sim N(0,R(k))$

非线性函数： $f\left ( \ast \right )$ 和 $h\left ( \ast \right )$

对于非线性函数，在状态预测传递时需要进行线性化，EKF在估计值处将非线性函数进行一阶泰勒级数展开，再利用线性的卡尔曼滤波技术进行滤波。滤波过程如下：

第一步：滤波初始化 $X_{0}$ 、 $P_{0}$

第二步：状态预测： $X(k+1\mid k) = f(X(k))$

第三步：观测预测： $Z(k+1\mid k) = h(X(k+1\mid k))$

第四步：计算线性化的状态转移矩阵：

$F\left ( k +1\right ) = \frac{\partial f}{\partial X}=\begin{bmatrix} \frac{\partial f_{1}}{\partial x_{1}} & \cdots &\frac{\partial f_{1}}{\partial x_{n}} \\ \vdots &\ddots &\vdots \\ \frac{\partial f_{n}}{\partial x_{1}}& \cdots & \frac{\partial f_{n}}{\partial x_{n}} \end{bmatrix}$

第五步：计算线性化的观测矩阵：

$H\left ( k+1 \right ) = \frac{\partial h}{\partial X}=\begin{bmatrix} \frac{\partial h_{1}}{\partial x_{1}} & \cdots &\frac{\partial h_{1}}{\partial x_{n}} \\ \vdots &\ddots &\vdots \\ \frac{\partial h_{n}}{\partial x_{1}}& \cdots & \frac{\partial h_{n}}{\partial x_{n}} \end{bmatrix}$

第六步：计算误差预测协方差矩阵：

$P\left ( k+1\mid k \right ) = F\left ( k+1 \right )P\left (k \right ){F\left ( k+1 \right )}'+Q\left ( k +1\right )$

第七步：计算卡尔曼增益：

$K\left ( k+1 \right ) = P\left ( k+1\mid k \right )H^{T}\left ( k+1 \right )\left [ H\left ( k+1 \right )P\left ( k+1\mid k \right )H^{T}\left ( k+1 \right ) +R(k+1)\right ]^{-1}$

第八步：计算状态更新值

$X\left ( k+1 \right ) = X\left ( k+1\mid k \right )+K\left ( k+1 \right )\left [ Z(k+1)-Z(k+1\mid k ) \right ]$

第九步：计算误差协方差更新值

$P\left ( k+1 \right ) = \left [ I - K\left ( k+1 \right ) H\left ( k+1 \right )\right ]P\left ( k+1\mid k \right )$

对应示例代码为（此实例中状态转移方程为线性方程，未计算线性化状态转移方程）：

%拓展卡尔曼滤波算法
 X_pre = F*X;
 P_pre = F*P*F' + Q;
 H = [2*X_pre(1,1), 0];
 K = P_pre*H'*inv(H*P_pre*H'+R);
 X_output = X_pre+ K*(Z - h(X_pre(1,1)));
 P_output = (eye(2)-K*H)*P_pre;

（二）简单凸组合融合

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

假设存在n个传感器。每个传感器的状态估计值为 $\widehat{x}_{i}$ ，误差协方差矩阵为 $\widehat{P}_{i}$ ,则对应的状态估计值和系统误差为：

系统误差： $P_{sf} = (P_{1}^{-1}+P_{2}^{-1}+\cdots +P_{n}^{-1})^{-1} = (\sum_{i=1}^{n}P_{i}^{-1})^{-1}$

状态估计值： $\widehat{x}_{sf} =P_{sf} (P_{1}^{-1}\widehat{x}_{1}+P_{2}^{-1}\widehat{x}_{2}+\cdots +P_{n}^{-1}\widehat{x}_{n}) =P_{sf} (\sum_{i=1}^{n}P_{i}^{-1}\widehat{x}_{i})$

二、模型构建

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

假设系统为一维线性变化的向量，状态向量由二维变量组成，分别为位置、速度。因此状态模型为线性模型，可以表示为：

状态转移方程为： $X(k+1) = F*X(k) + w(k)$

状态转移矩阵： $F=\begin{bmatrix} 1 &1 \\ 1 & 0 \end{bmatrix}$

过程噪声： $w(k)\sim N(0,Q(k))$

之后，状态向量经过非线性变换之后，附加上系统噪声就得到了系统的观测值。

观测方程： $Z(k) = h(X(k)) + v(k)$

非线性观测函数： $h=\left [ X(k)\right ]^{2}$

观测噪声： $v(k)\sim N(0,R(k))$

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

clc;
% clear ;
close all;
%%
%定义参数
B = 10;
M = 1;
N_end = 100;%结束点数
N_start =2;%开始点数
x= N_start:1:N_end;
[~,N] = size(x);
Z_cal = zeros(B,N);
nf = 0.0000001;
%定义噪声矩阵
Q = cell(1,N);
Q(:) = {zeros(2,2)};

for i=1:B
    Q{i} = nf*diag([1,1]);
end
R = 10000*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)  =  [N_start,1];
        else
            X_data(:,i)  =  F*X_data(:,i-1);
        end
    end
    X_data = X_data + sqrt(Q{j})*randn(2,N);
    Z_cal(j,:) = X_data(1,:).^2+sqrt(R(j))*randn(1,N);
end

（二）单个滤波器仿真

建立系统模型之后，对单个滤波器进行滤波迭代。首先，设定仿真初值，假设状态初值和协方差初值为：

$X_{0} = [2,1]^{T}$ ； $P_{0} = diag([1 , 1])$

对应的滤波程序为：

%单个传感器滤波
X_all = zeros(B,N);
P_all = zeros(B,N);
for i =1:B
    [X_all(i,:),P_all(i,:)] = filter_fusion(Z_cal(i,:),R(1,i),F,Q{i});
end

其中“filter_fusion（）”函数为自定义函数，可以对10个滤波器的原始数据进行批量处理，对应代码为：

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

（三）融合滤波

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

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

三、结果展示

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

figure(1)
plot(Z_cal');
title('原始数据','FontSize',12);

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

figure(2)
plot(1:N,sqrt(Z_cal(5,:)));
hold on;
plot(1:N,X_all(5,:));
hold on;
plot(1:N,X_EK);
legend('单个滤波器','融合','FontSize',12);

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

figure(3)
error_obser = sqrt(Z_cal(5,:))-x;
error_single = X_all(5,:)-x;
error_fusion = X_EK-x;
plot(1:N,error_obser,'LineWidth',1);
hold on;
plot(1:N,error_single,'LineWidth',1);
hold on;
plot(1:N,error_fusion,'LineWidth',1);
legend('观测误差','单个滤波器误差','融合误差','FontSize',12);

拓展卡尔曼滤波（EKF）在噪声强度系数 $nf$ 较小的情况（即过程噪声较小的情况下）能够有效滤除观测噪声，提升滤波精度。但是在过程噪声较大的情况下，EKF几乎没有什么效果，这是因为随机噪声过大会导致“无规律可循”，进而导致系统无法滤除噪声。从最后一张图中可以看出，单个滤波的误差小于观测数据误差，证明滤波算法有效。并且融合后的滤波误差小于单个滤波器的误差，证明融合算法有效。仿真结果表明，所提融合滤波算法能够实现有效滤波跟踪。

总结

以上就是今天要讲的内容，本文介绍了一种多传感器拓展卡尔曼滤波（EKF）算法，结果表明滤波算法能够实现有效滤波跟踪，并且相比于单个传感器滤波，多传感器融合能够有效提升滤波精度。

滤波~

关注

28
点赞
踩
48

收藏

觉得还不错? 一键收藏
0
评论
（二）多传感器拓展卡尔曼滤波（EKF）算法

本文介绍了一种用于多传感器的拓展卡尔曼滤波（EKF）算法。首先，介绍了EKF中运用到的一阶泰勒展开技术，介绍了EKF的本质，展示了滤波过程。之后，对于多传感器状态值估计中用到的简单凸组合技术进行了讲解。最后，结合一个实例和matlab程序对算法的具体实现过程进行了讲解。仿真结果证明了滤波融合算法的有效性和实用性。（可在资源中下载）
复制链接

扫一扫