(二)多传感器拓展卡尔曼滤波(EKF)算法

这篇具有很好参考价值的文章主要介绍了(二)多传感器拓展卡尔曼滤波(EKF)算法。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

目录

前言

一、基础知识

(一)拓展卡尔曼滤波

(二)简单凸组合融合

二、模型构建

(一)状态和观测模型构建

(二)单个滤波器仿真

(三)融合滤波

三、结果展示

总结


前言

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


一、基础知识

(一)拓展卡尔曼滤波

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

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

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

ekf算法,滤波算法仿真与实践,算法

ekf算法,滤波算法仿真与实践,算法

        过程噪声:  

        观测噪声:

        非线性函数:和

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

        第一步:滤波初始化、

        第二步:状态预测:ekf算法,滤波算法仿真与实践,算法

        第三步:观测预测:ekf算法,滤波算法仿真与实践,算法

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

ekf算法,滤波算法仿真与实践,算法

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

    ekf算法,滤波算法仿真与实践,算法

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

ekf算法,滤波算法仿真与实践,算法

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

ekf算法,滤波算法仿真与实践,算法

        第八步:计算状态更新值

ekf算法,滤波算法仿真与实践,算法

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

ekf算法,滤波算法仿真与实践,算法

        对应示例代码为(此实例中状态转移方程为线性方程,未计算线性化状态转移方程):

%拓展卡尔曼滤波算法
 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个传感器。每个传感器的状态估计值为,误差协方差矩阵为,则对应的状态估计值和系统误差为:

        系统误差: ekf算法,滤波算法仿真与实践,算法

        状态估计值:ekf算法,滤波算法仿真与实践,算法

二、模型构建

(一)状态和观测模型构建

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

        状态转移方程为:ekf算法,滤波算法仿真与实践,算法

        状态转移矩阵:

        过程噪声:

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

        观测方程:ekf算法,滤波算法仿真与实践,算法

        非线性观测函数:

        观测噪声:

        此处假设有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_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);

        ekf算法,滤波算法仿真与实践,算法

        将第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);

ekf算法,滤波算法仿真与实践,算法        

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

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算法,滤波算法仿真与实践,算法    

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


总结

        以上就是今天要讲的内容,本文介绍了一种多传感器拓展卡尔曼滤波(EKF)算法,结果表明滤波算法能够实现有效滤波跟踪,并且相比于单个传感器滤波,多传感器融合能够有效提升滤波精度。文章来源地址https://www.toymoban.com/news/detail-860594.html

到了这里,关于(二)多传感器拓展卡尔曼滤波(EKF)算法的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 卡尔曼滤波(KF)和扩展卡尔曼滤波(EKF)相应推导

    从上个世纪卡尔曼滤波理论被提出,卡尔曼滤波在控制论与信息论的连接上做出了卓越的贡献。为了得出准确的下一时刻状态真值,我们常常使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波等等方法,这些方法在姿态解算、轨迹规划等方面有着很多用途。卡尔

    2024年02月03日
    浏览(61)
  • 基于卡尔曼滤波的视频跟踪,基于卡尔曼滤波的运动小球跟踪

    完整代码和数据下载链接:基于卡尔曼滤波的视频跟踪,基于卡尔曼滤波的运动小球跟踪(代码完整,数据齐全)资源-CSDN文库 https://download.csdn.net/download/abc991835105/88738577 卡尔曼滤波原理 RBF的定义 RBF理论 易错及常见问题 RBF应用实例,基于rbf的空调功率预测 代码 结果分析

    2024年02月02日
    浏览(46)
  • 卡尔曼滤波理论小释之卡尔曼增益

    卡尔曼增益是卡尔曼滤波理论中的一个核心概念。一般教材里面是这么给出它的公式的: 图1  卡尔曼增益 直觉上容易理解,所谓的增益是指每次融合数据后不确定性的变化程度。如果融合了新的数据后不确定性降低了,那么这个增益就是正面的,有助于提高预测的准确度。

    2024年02月05日
    浏览(83)
  • 卡尔曼滤波器-概述及用递归思想解读卡尔曼滤波器 | 卡尔曼滤波器应用举例(附Matlab程序)| 数学基础-数据融合、协方差矩阵、状态空间方程

      卡尔曼滤波器是最优化的(Optimal)、递归的(Recursive)、数字处理的(Data Processing)算法(Algorithm)。卡尔曼滤波器更像是观测器,而不是一般意义上的滤波器,应用广泛,尤其是在导航中,它的广泛应用是因为生活中存在大量的不确定性。   当描述一个系统的不确

    2024年02月06日
    浏览(49)
  • 卡尔曼滤波系列_实例(二)均加速运动的卡尔曼滤波

    此系列(一)对卡尔曼滤波的原理进行了简单的阐述,总结了卡尔曼滤波的两大过程:预测和更新。接下来举例对卡尔曼滤波的使用进行介绍,加深对卡尔曼滤波的理解。 1.场景介绍 如上图所示,可知小车的初始速度为0,初始位置也为0,小车向前的加速度为1,小车感知自身

    2024年02月15日
    浏览(36)
  • 卡尔曼滤波学习笔记

    从直观上来看,卡尔曼滤波是把两个存在误差的结果 融合 在一起,得到一个从数学上可以得到证明的 最优估计值 。 而这两个存在误差的结果,一个是从理论上推导出来的,称之为 先验估计值 ;一个是用传感器测量出来的,称之为 测量值 。它们之所以存在误差,是因为前

    2024年02月11日
    浏览(42)
  • 卡尔曼滤波介绍

        卡尔曼滤波无论是在单目标还是多目标领域都是很常用的一种算法,将卡尔曼滤波看作一种运动模型,用来对目标的位置进行预测,并且利用预测结果对跟踪的目标进行修正,属于自动控制理论中的一种方法。     在对视频中的目标进行跟踪时,当 目标运动速度较慢 时

    2024年02月14日
    浏览(38)
  • 【算法系列】卡尔曼滤波算法

    ·【算法系列】卡尔曼滤波算法 ·【算法系列】非线性最小二乘求解-直接求解法 ·【算法系列】非线性最小二乘求解-梯度下降法 ·【算法系列】非线性最小二乘-高斯牛顿法  ·【算法系列】非线性最小二乘-列文伯格马夸尔和狗腿算法  文章目录 系列文章 文章目录 前言 一、

    2024年02月12日
    浏览(38)
  • 算法介绍及实现——卡尔曼滤波

            本文主要介绍卡尔曼滤波的推导过程及建模步骤,是网站的学习笔记。本文主要是通过例子来引出卡尔曼滤波的建模思想及算法步骤。 参考网站:(这个网站讲得真的很详细很清楚,层层递进,逻辑清晰) Kalman Filter Tutorial https://www.kalmanfilter.net/ 目录 一、引言

    2024年02月14日
    浏览(41)
  • 卡尔曼滤波算法原理及示例

      例程:物体做匀速运动每秒运动1m,观测器观测方差为1m

    2024年02月15日
    浏览(39)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包