(三)多传感器平方根容积卡尔曼滤波(SRCKF)算法

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

目录

前言

一、基础知识

(一)平方根容积卡尔曼滤波

(二)简单凸组合融合

二、模型构建

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

(二)单个滤波器仿真

(三)融合滤波

三、结果展示

总结


前言

        本博客介绍了一种用于多传感器的平方根容积卡尔曼滤波(SRCKF)算法。首先,介绍了SRCKF的原理及滤波过程。之后,对于多传感器状态值估计中用到的简单凸组合技术进行了讲解。最后,结合一个实例和matlab程序对算法的具体实现过程进行了讲解。仿真结果证明了滤波融合算法的有效性和实用性。


一、基础知识

(一)平方根容积卡尔曼滤波

        常用的卡尔曼滤波算法仅能对线性高斯模型做出最优状态估计。实际应用中会存在很多非线性函数,比如平方函数、指数函数以及三角函数等,这就导致普通卡尔曼滤波算法已经不适用。解决非线性滤波问题需要计算条件后验概率分布,但是计算度十分复杂。因此人们提出各种次优方法来解决这个问题,上篇文章中提到的拓展卡尔曼滤波(EKF)算法核心就是利用一阶泰勒技术将非线性问题线性化。但是对于更高精度的非线性滤波问题,EKF的误差太大,可能会使得滤波发散。并且EKF需要计算雅可比行列式,实际情况中很难得到系统的雅可比行列式。因此提出一种基于容积原则的数值积分方法,来计算非线性变换之后的均值和协方差。即容积卡尔曼滤波方法(CKF),又因为平方根滤波的鲁棒性特点,本博客采用平方根容积卡尔曼滤波(SRCKF)方法。

        需要注意的一点是SRCKF中状态误差矩阵的传递是基于状态误差协方差的乔列斯基分解因子进行的,不同常规卡尔曼滤波。本博客注重方法的应用,只对于原理做简单介绍。


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

srckf 算法,滤波算法,算法

srckf 算法,滤波算法,算法

        过程噪声:  

        观测噪声:

        非线性函数:和

        在讲述滤波过程之前,需要先定义基本的容积点和对应权值。首先。使用三阶容积原则获取的基本容积点和对应的权值为:

        其中,,表示容积点数量;表示状态向量维数;表示n维单位向量全排列之后的完整全对称点集,表示点集中的第个点。


        滤波过程如下:

        第一步:滤波初始化、

        第二步:时间更新:

        1.计算容积点:srckf 算法,滤波算法,算法

        2.计算通过状态方程传播的容积点:srckf 算法,滤波算法,算法

        3.计算状态预测和方差预测的乔列斯基分解因子:

srckf 算法,滤波算法,算法

srckf 算法,滤波算法,算法

        其中,表示矩阵进行三角化,矩阵srckf 算法,滤波算法,算法的定义如下:

srckf 算法,滤波算法,算法

        第三步:观测更新:

        1.计算容积点:srckf 算法,滤波算法,算法

        2.计算通过非线性观测函数传递的容积点:srckf 算法,滤波算法,算法

        3.计算观测的预测、新息方差的乔列斯基分解因子和协方差:

srckf 算法,滤波算法,算法

srckf 算法,滤波算法,算法

srckf 算法,滤波算法,算法

        其中,srckf 算法,滤波算法,算法srckf 算法,滤波算法,算法分别为:

srckf 算法,滤波算法,算法

srckf 算法,滤波算法,算法

        第四步:状态和方差的乔列斯基分解因子更新:

srckf 算法,滤波算法,算法

srckf 算法,滤波算法,算法

srckf 算法,滤波算法,算法


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

%平方根容积卡尔曼滤波算法
chol_Q=blkdiag(chol(Q,'lower'));
n=size(X,1);
m=2*n;
kkss=[eye(n);-eye(n)]'*sqrt(n);
for i=1:m
    Xix(:,i)=F*(S*kkss(:,i)+X);
end
Xd=sum(Xix,2)/m;
Xx=(Xix-repmat(Xd,1,m))/sqrt(m);
[~,Sda]=qr([Xx,chol_Q]');
Sda=(Sda(1:n,1:n))';
for i=1:m
    Xi(:,i)=Sda*kkss(:,i)+Xd;
end  
for i =1:m
    Zi(1,i)=h(Xi(:,i),A);
end

(二)简单凸组合融合

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

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

        系统误差: srckf 算法,滤波算法,算法

        状态估计值:srckf 算法,滤波算法,算法

二、模型构建

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

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

        状态转移方程为:srckf 算法,滤波算法,算法

        状态转移矩阵:

        过程噪声:

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

        观测方程:srckf 算法,滤波算法,算法

        非线性观测函数:

        观测噪声:

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

clc;
clear all;
close all;
%%
B = 10;  %陀螺仪个数
M = 1;   %状态维数
N_end = 100;%结束点数
N_start = 5;%开始点数
x= N_start:0.5:N_end;
[~,N] = size(x);
Z_1 = zeros(B,N);
nf = 0.0001;%噪声强度
Q = cell(1,N);
Q(:) = {zeros(2,2)};
for i=1:B
Q{i} = nf*diag([1,1]);
end
R = 1000*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,0.5]';
        else
            X_data(:,i)  =  F*X_data(:,i-1);
        end
    end
    X_data = X_data + sqrt(Q{j})*randn(2,N);
    Z_1(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个滤波器的原始数据进行批量处理。

(三)融合滤波

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

%信息融合
X_F = zeros(M,N);
P_F = zeros(M,N);
for i = 1:N
    X_mid = X_all(:,i);
    P_mid = P_all(:,i);
    %融合
    P_F_inv = 0;
    X_c = 0;
    for j = 1:B
        P_F_inv = (eye(M)/P_mid(j)) + P_F_inv ;
        X_c = (eye(M)/P_mid(j))*X_mid(j) + X_c;
    end
    P_F(1,i) = eye(M)/P_F_inv;
    X_F(1,i) =(eye(M)/P_F_inv)*X_c;
end

三、结果展示

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

figure(1)
plot(Z_1');
title('原始数据','Fontsize',12);
xlim([1,N]);

   srckf 算法,滤波算法,算法

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

figure(2)
plot(1:N,sqrt(Z_1(5,:)));
hold on;
plot(1:N,X_all(5,:));
hold on;
plot(1:N,X_F);
legend('观测','单个滤波','融合滤波','Fontsize',12);
xlim([1,N]);

srckf 算法,滤波算法,算法       

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

figure(3)
error_obser = sqrt(Z_1(5,:))-x;
error_single = X_all(5,:)-x;
error_fusion = X_F-x;
plot(1:N,error_obser);
hold on;
plot(1:N,error_single);
hold on;
plot(1:N,error_fusion);
legend('观测','单个滤波','融合滤波','Fontsize',12);
xlabel('x');
xlim([1,N]);

srckf 算法,滤波算法,算法       

        在状态维数较高的情况下,平方容积卡尔曼滤波(SRCKF)的滤波精度相比与其他滤波方法更高。所以在状态维数比较高的非线性滤波情况下,建议使用SRCKF进行滤波。从最后一张图中可以看出,滤波误差不断减小,说明滤波收敛。并且单个滤波的误差小于观测数据误差,证明滤波算法有效。同时融合后的滤波误差小于单个滤波器的误差,证明融合算法有效。仿真结果表明,所提融合滤波算法能够实现有效滤波跟踪。


总结

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

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

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

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

相关文章

  • 基于EKF扩展卡尔曼滤波的传感器网络目标跟踪matlab仿真

    目录 1.算法运行效果图预览 2.算法运行软件版本 3.部分核心程序 4.算法理论概述 5.算法完整程序工程 MATLAB2022a         随着传感器网络技术的不断发展,目标跟踪作为其核心应用之一,在军事、民用等领域中得到了广泛的关注。扩展卡尔曼滤波(EKF)作为一种有效的非线性滤

    2024年02月22日
    浏览(72)
  • leetcode69---x 的平方根

    大家好,我是大唐,刚刷完了几道经典的leetcode题,今天给大家分享一道leetcode上面的二分查找经典题型---x 的平方根,我们往下看。 给你一个非负整数  x  ,计算并返回  x  的  算术平方根  。 由于返回类型是整数,结果只保留  整数部分  ,小数部分将被  舍去 。 注意

    2024年03月18日
    浏览(44)
  • Python算法例4 求平方根

    实现int sqrt(int x)函数,计算并返回x的平方根。 sqrt(3)=1;sqrt(4)=2;sqrt(5)=2;sqrt(17)=4。 要实现计算整数x的平方根函数sqrt(x),可以使用二分查找法。 首先,我们定义一个变量left = 0用来表示搜索区间的左边界,以及一个变量right = x用来表示搜索区间的右边界。初

    2024年02月05日
    浏览(39)
  • leetcode69 x 的平方根

    题目变形为找到 f ( x ) = x 2 − c = 0 f(x)=x^2-c=0 f ( x ) = x 2 − c = 0 的根,其中 x x x 是非负整数。由于 f ( 0 ) = − c ≤ 0 , f ( c ) = c 2 − c ≥ 0 f(0)=-cle0,f(c)=c^2-cge0 f ( 0 ) = − c ≤ 0 , f ( c ) = c 2 − c ≥ 0 ,则 [ 0 , c ] [0,c] [ 0 , c ] 之间必然存在一个根,使用二分法。 但是由于计算

    2024年02月22日
    浏览(42)
  • FPGA verilog 简单的平方根求法

    用下面的平方根求法不需要乘法,只需简单的移位就能实现。 原理参照论文 A New Non-Restoring Square Root Algorithm and Its VLSI Implementations

    2024年02月04日
    浏览(35)
  • C语言—求平方根(sqrt函数)

            在数学当中,我们知道了平方根。那么在C语言当中求一个数的平方根是如何实现的呢?今天我们就来讲解。  sqrt()函数为库函数,所以要包含对应的头文件,这个头文件包含了sqrt()函数的定义  下图中,x为要计算平方根的参数,sqrt()函数返回的是x的平方根,返回值

    2024年01月22日
    浏览(36)
  • LeetCode每日一题——x 的平方根

    乍一看题目只需要算一个数的平方根,根据我们之前学的C语言我们能很快的想到使用sqrt,pow这类的math.h库函数,但是题目要求我们不能使用,那么我们便可以使用我们的数学思想,将给的整数拆成两个一样的数相乘。 代码实现: 运行结果:   PS:看到这里了,码字不易,给

    2024年03月23日
    浏览(48)
  • 数值分析——改进的平方根法(matlab实现)

    最近上数值分析学到了改进平方根法的原理,并最终借助matlab实现了运用该方法进行解题,浅浅的记录一下。 由于本人并非数学专业,不擅长公式的推导,在此仅将书中内容拍照整理,供大家参考,主要用的是图中圈的两个公式: 式中的D是正定矩阵,求解过程参考第一张图

    2024年02月11日
    浏览(114)
  • Java习题之实现平方根(sqrt)函数

    目录 前言 二分查找 牛顿迭代法 总结 🎁博主介绍:博客名为tq02,已学C语言、JavaSE,目前学了MySQL和JavaWeb 🎥学习专栏:  C语言        JavaSE      MySQL基础 🎄博主链接:tq02的博客_CSDN博客-C语言,Java,MySQL领域博主         可使用java.lang.Math类的 sqrt(double)方法 求平方根。

    2024年02月15日
    浏览(44)
  • 【算法专题突破】二分查找 - x 的平方根(18)

    目录 1. 题目解析 2. 算法原理 3. 代码编写 写在最后: 题目链接:69. x 的平方根 - 力扣(LeetCode) 这道题就是求算数平方根, 要注意的点是他只需要保留整数部分,小数部分会舍去 我们确定好一个区间 1 ~ x,数字 x 的算数平方根一定在这里面, 最简单的思路就是用暴力解法

    2024年02月07日
    浏览(39)

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

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

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

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

二维码1

领取红包

二维码2

领红包