基于先验信息的协方差矩阵重构抗干扰算法

卡尔曼滤波_卡尔曼算法1.引言1.1 概述卡尔曼滤波是一种用于估计系统状态的技术，通过融合传感器测量值和系统模型的预测值，提供对系统状态的最优估计。

它的应用十分广泛，特别在导航、图像处理、机器人技术等领域中发挥着重要作用。

在现实世界中，我们往往面临着各种噪声和不确定性，这些因素会影响我们对系统状态的准确估计。

卡尔曼滤波通过动态调整系统状态的估计值，可以有效地抑制这些干扰，提供更加精确的系统状态估计。

卡尔曼滤波的核心思想是利用系统模型的预测和传感器测量值之间的线性组合，来计算系统状态的最优估计。

通过动态地更新状态估计值，卡尔曼滤波可以在对系统状态的准确估计和对传感器测量值的实时响应之间进行平衡。

卡尔曼滤波算法包括两个主要步骤：预测和更新。

在预测步骤中，通过系统模型和上一时刻的状态估计值，预测当前时刻的系统状态。

在更新步骤中，将传感器测量值与预测值进行比较，然后根据测量误差和系统不确定性的权重，计算系统状态的最优估计。

卡尔曼滤波具有很多优点，例如它对传感器噪声和系统模型误差具有鲁棒性，可以提供较为稳定的估计结果。

此外，卡尔曼滤波还可以有效地处理缺失数据和不完全的测量信息，具有较高的自适应性和实时性。

尽管卡尔曼滤波在理论上具有较好的性能，但实际应用中还需考虑诸如系统模型的准确性、测量噪声的特性等因素。

因此，在具体应用中需要根据实际情况进行算法参数的调整和优化，以提高估计的准确性和可靠性。

通过深入理解卡尔曼滤波的原理和应用，我们可以更好地应对复杂环境下的估计问题，从而在实际工程中取得更好的效果。

本文将介绍卡尔曼滤波的基本原理和算法步骤，以及其在不同领域的应用案例。

希望通过本文的阅读，读者们可以对卡尔曼滤波有一个全面的了解，并能够在实际工程中灵活运用。

1.2文章结构文章结构部分的内容可以按照以下方式编写：1.2 文章结构本文将围绕卡尔曼滤波和卡尔曼算法展开论述。

首先，我们会在引言部分对卡尔曼滤波和卡尔曼算法进行简要概述，介绍其基本原理和应用领域。

OpenMV卡尔曼滤波算法简介卡尔曼滤波算法是一种用于估计系统状态的优化算法，它通过结合测量值和系统模型，提供最优估计值。

OpenMV是一个基于MicroPython的开源计算机视觉平台，它提供了一系列图像处理和机器视觉功能。

在OpenMV中使用卡尔曼滤波算法可以提高图像处理和目标跟踪的准确性。

本文将介绍卡尔曼滤波算法的原理和应用，以及在OpenMV中如何实现卡尔曼滤波算法。

卡尔曼滤波算法原理卡尔曼滤波算法基于状态空间模型，它将系统的状态表示为一个向量，通过测量值和系统模型进行状态估计。

卡尔曼滤波算法的核心思想是通过融合先验信息（系统模型）和后验信息（测量值）来提供最优估计。

卡尔曼滤波算法包括两个主要步骤：预测和更新。

预测步骤在预测步骤中，根据系统模型和先验状态估计值，通过状态转移方程预测下一时刻的状态。

状态转移方程可以表示为：x(k) = F * x(k-1) + w(k-1)其中，x(k)是当前时刻的状态向量，F是状态转移矩阵，w(k-1)是过程噪声。

预测步骤还需要计算协方差矩阵P(k)，它表示状态估计的不确定性。

协方差矩阵的预测可以通过以下公式计算：P(k) = F * P(k-1) * F^T + Q其中，P(k-1)是上一时刻的协方差矩阵，Q是过程噪声的协方差矩阵。

更新步骤在更新步骤中，根据测量值和预测的状态估计值，通过测量方程融合测量值和先验信息，得到更新后的状态估计值。

测量方程可以表示为：z(k) = H * x(k) + v(k)其中，z(k)是当前时刻的测量值，H是测量矩阵，v(k)是测量噪声。

更新步骤还需要计算卡尔曼增益K(k)，它表示预测值和测量值之间的权重。

卡尔曼增益的计算可以通过以下公式得到：K(k) = P(k) * H^T * (H * P(k) * H^T + R)^-1其中，R是测量噪声的协方差矩阵。

最后，通过以下公式更新状态估计值和协方差矩阵：x(k) = x(k) + K(k) * (z(k) - H * x(k))P(k) = (I - K(k) * H) * P(k)卡尔曼滤波算法在OpenMV中的应用OpenMV提供了一系列图像处理和机器视觉功能，包括目标检测、目标跟踪和姿态估计等。

0引言随着雷达探测应用场景的拓展，各类电磁干扰与自然环境杂波干扰大量涌入，破坏雷达对目标信号的检测与识别，因此要求雷达具有灵活的分辨能力和抗干扰能力。

为了解决复杂电磁环境下的干扰全渗透问题，雷达采用了频率捷变、副瓣对消、副瓣匿影等抗干扰手段，用以解决副瓣干扰，而当干扰从雷达天线主瓣进入时，雷达接收机噪声水平增大，信噪比降低。

利用和差波束的主瓣对消可以抑制近主瓣干扰，但需要将天线主波束对准目标，这在干扰环境下很难实现。

通过对脉冲宽度和强度进行判别的窄脉冲剔除，针对密集假目标这类欺骗式干扰效果明显。

但是，面对非合作目标周边存在的伴随式干扰，雷达将无法跟踪和识别主目标，从而严重影响雷达识别目标的能力。

盲源分离是上世纪八十年代发展起来的一种信号处理技术，原指在缺乏源信号和信道参数先验知识的情况下，仅凭传感器就能观测、分离独立目标信号。

这一技术在无线通信、语音识别、生物医学和水声信号处理等方面得到了广泛的关注和应用研究，在雷达中也有一定的应用前景。

本文主要研究盲源分离算法在阵列雷达抗主瓣干扰中的应用，利用目标回波信号与干扰信号的角度微小差异，实现从多个通道接收的混合信号中分离出干扰和目标，切实提升雷达检测性能。

1信号模型盲源分离需要利用多个通道接收目标回波和干扰的混合信号，可以将雷达阵列接收信号表示为：X=AS+N （1）其中X ∈C m ×n 为接收信号矩阵，A ∈C m ×p 为未知混合矩阵，S ∈C p ×n 为源信号矩阵，N ∈C m ×n 为通道噪声，m 为通道数，n 为信号长度，p 为信号数，各信号在时域和频域均重叠。

盲源分离算法不要求主波束的最大点指向目标或干扰方向，只要目标和干扰方向存在差异即可，其主要原理就是从接收到的未知混合信号中求解到分离矩阵B ，使得Y=BX ，其中Y 近似于源信号S ，从而实现信号的识别与分离。

值得注意的是，进行盲源分离通常存在以下约束条件：①通道数m 不少于信号数p ；②源信号之间相互统计独立；③源信号各矢量均值为0，至多有一个是高斯信号。

卡尔曼平滑算法
卡尔曼平滑（Kalman Smoothing）算法是一种基于状态空间模
型的滤波算法，用于估计系统的状态。

它使用系统的观测值和先验知识，通过迭代更新估计值，提供更精确的状态估计。

卡尔曼平滑算法的基本原理如下：
1. 初始化：根据系统的先验知识，初始化系统的状态估计值和协方差矩阵。

2. 预测：根据系统的动态模型，预测下一时刻的状态估计值和协方差矩阵。

3. 更新：根据系统的观测模型，计算当前时刻的测量残差，并更新状态估计值和协方差矩阵。

4. 平滑：从后向前，根据当前时刻的状态估计值和协方差矩阵，通过估计状态估计值和协方差矩阵的条件分布，计算过去时刻的状态估计值和协方差矩阵。

卡尔曼平滑算法的优点是能够处理线性动态系统，并且具有最小均方误差性质。

它还可以通过迭代计算，提供更准确、稳定的状态估计结果。

然而，该算法在处理非线性系统和大量观测噪声时可能存在一些问题，需要使用扩展卡尔曼滤波（Extended Kalman Filter）或无迹卡尔曼滤波（Unscented Kalman Filter）等扩展方法进行改进。

SAR干涉图降噪的稳健协方差矩阵分解法赵超英;王宝行【摘要】Interferogram denoising plays an important role to the application of InSAR technique.If the phase noise cannot be well filtered, the phase unwrapping error is frequently arisen, which will further result in the mistakes in the DEM product and the deformation result.The complex value of each SAR resolution unit is superimposed by the phases from different scatterers, so the paper focuses on the characteristics of single dominant phase scattering model (the permanent scatterer) and traditional distributed scatterer of single scattering mechanism.Then the robust covariance matrix, estimated based on multi-baseline SAR data, is decomposed and the eigenvector corresponding to the maximum eigenvalue is chosen as the filtered phase.Besides, the covariance matrix is robustly estimated by weighted averaging the heterogeneous points.This method can operate better than the improved Goldstein filter algorithm in the terms of coherence improvement and effective coherent targets increasing, especially in the low-coherence regions.Eight real TerraSAR-X data over one land subsidence region, Qingxu, Shanxi verifies the advantages of our new method.%干涉图降噪在InSAR技术应用中发挥着重要作用, 若降噪效果不好将引起干涉图相位解缠的误差, 并进一步导致DEM或形变结果的错误.由于干涉图分辨单元的信号 (相位) 是由分辨单元内多个散射体的回波信号 (相位) 叠加而成, 本文针对单一主导散射体的散射模型 (永久性散射体模型) 和只考虑一种散射机制的分布式散射体模型相位的特点, 对多基线SAR数据估计的协方差矩阵采用特征值分解的方法来分离相位中的噪声, 通过提取最大特征值对应的特征向量 (相位) , 从而实现干涉图降噪的目的.而对于协方差矩阵估计时引入的异质点, 本文采用了一种稳健的协方差矩阵估计方法.通过覆盖山西清徐地面沉降形变区的8景真实TerraSAR数据试验验证了该方法的有效性.结果表明该方法比改进的Goldstein滤波方法在相干性提高、有效目标点增加两方面均有显著提高, 特别在低相干区域由于相干点的增加也获取了更多的形变监测信息.【期刊名称】《测绘学报》【年(卷),期】2019(048)001【总页数】10页(P24-33)【关键词】同质点;稳健估计;协方差矩阵分解;干涉图去噪【作者】赵超英;王宝行【作者单位】长安大学地质工程与测绘学院,陕西西安 710054;地理国情监测国家测绘地理信息局工程技术研究中心,陕西西安 710054;长安大学地质工程与测绘学院,陕西西安 710054【正文语种】中文【中图分类】P237合成孔径雷达干涉测量(synthetic aperture radar interferometry，InSAR)已广泛应用于火山、冰川、地面沉降、地震和滑坡等多类地表形变的监测中，成为现代地球学科研究的重要技术之一。

GPS接收机在高动态下的抗干扰技术实现周博海;郑建生;陈鲤文;朱玉建【摘要】针对GPS接收机的功率倒置算法(PI)在高动态环境下的加权系数更新速度滞后于干扰入射的变化速度问题,提出基于零陷加宽的PI功率倒置算法,通过零陷的加宽对来向干扰的接触面积更大,能抑制干扰的角度范围更大.同时为了满足硬件平台的可行性,改进了基于零陷加宽的PI功率倒置算法,对协方差矩阵引入了与零陷宽度相关的矩阵T,一次权值更新只需计算一次协方差矩阵.仿真结果表明在高动态环境下,抗干扰的零陷增益达到68 dB.在DSP+ FPGA的硬件平台上,一次权值更新时间只需6.3 ms,满足高动态的干扰入射角度的变化速度.【期刊名称】《科学技术与工程》【年(卷),期】2015(015)021【总页数】6页(P136-141)【关键词】GPS接收机;高动态;抗干扰;功率倒置算法;零陷加宽【作者】周博海;郑建生;陈鲤文;朱玉建【作者单位】武汉大学卫星导航定位技术研究中心,武汉430079;武汉大学卫星导航定位技术研究中心,武汉430079;武汉大学电子信息学院,武汉430079;武汉大学卫星导航定位技术研究中心,武汉430079【正文语种】中文【中图分类】TN911.X目前地面上常用的GPS干扰抑制方法有时域滤波、空域滤波和空时自适应滤波技术(STAP)。

时域自适应ATF(mayflower adaptive temporal filter)滤波器芯片，它能有效抑制大于30 dB的高动态干扰。

空域滤波利用改变天线阵指向使得干扰来向上的增益最小，从而达到干扰有效抑制的效果。

自适应天线能使GPS接收机的干扰抑制能力提高40～50 dB。

目前高增益GPS接收机，其空域抑制高动态干扰的能力很强，天线阵列个数为16个。

空时自适应滤波技术通过空域和时域分别对来向干扰信号进行自适应处理，这样在不增加天线阵元个数的情况下可以有效增加抑制干扰的个数，完全弥补了空域滤波的弱点。

一种改进的UKF-SLAM算法吕太之;周武;赵春霞【摘要】针对外部干扰导致基于无迹卡尔曼滤波的同时定位与地图创建(UKF-SLAM)算法精度降低甚至发散的问题,提出一种改进的UKF-SLAM算法.算法在系统状态更新之前引入外部干扰检测和状态方差膨胀,在干扰发生的周期内快速做出响应,提高了系统状态估计的精度.临近观测的对比检测不受累计误差的影响,提高了检测的精度,不同状态方差膨胀方式能够响应不同类型的外部干扰.仿真实验结果表明,在存在外部干扰的环境中,改进UKF-SLAM算法估计精度高于SMCI-SLAM和UKF-SLAM算法.【期刊名称】《中北大学学报（自然科学版）》【年(卷),期】2018(039)006【总页数】10页(P717-725,751)【关键词】移动机器人;同时定位与地图创建;无迹卡尔曼滤波;外部干扰【作者】吕太之;周武;赵春霞【作者单位】江苏海事学院信息工程学院,南京210017;南京理工大学计算机科学与技术学院,南京210094;浙江师范大学工学院,浙江金华321019;南京理工大学计算机科学与技术学院,南京210094【正文语种】中文【中图分类】TP2420 引言自21世纪初以来，国内外对机器人技术的发展越来越重视，使其成为当今前沿高新技术研究最活跃的领域之一[1]. 同时定位与地图创建(simultaneous localization and mapping, SLAM)具有重要的理论和应用价值，是实现移动机器人自主导航的关键. SLAM已经在移动机器人、无人车辆、无人水下航行器、无人机、增强现实等领域得到广泛应用[2-3]. 然而， SLAM技术仍然面临很多挑战，如复杂的大环境、可靠的数据融合、非线性和未知的先验知识等[4-6]. SLAM问题的常用概率解决方案有两类，基于卡尔曼滤波(Kalman Filter, KF)和基于粒子滤波(Particle Filter, PF)的SLAM算法[6]. 扩展卡尔曼滤波(Extended Kalman Filter, EKF)将非线性化的系统线性化，来应用于SLAM问题的解决. EKF-SLAM在机器人导航领域被广泛应用，相继在室内、结构化道路、水下等场合取得了成功. Bailey等人对EKF-SLAM算法进行了详细的论述和实验验证，同时对EKF-SLAM算法进行了优化，提高了状态向量维数很高时算法的效率[7]. 当路标数量较多时， EKF-SLAM计算量很大，同时由于EKF在线性化的过程中，不可避免的引入了线性化误差，这样状态向量的估计就难以达到最优. 为了降低计算复杂度和减少线性化误差，研究人员提出一系列改进方法. 降低计算复杂度的方法有不变扩展卡尔曼滤波(Invariant Extended Kalman Filter, IEKF)[8]、平均扩展卡尔曼滤波(Mean Extended Kalman Filter, MEKF)[9]等. 对于提高算法精确度方面主要从运动模型的改进和减少由线性化带来的误差着手. 无迹卡尔曼滤波(Unscented Kalman Filter, UKF)使用UT(Unscented Transform)变换通过Sigma点重构出状态向量的统计特性(均值和误差)，为非线性化的SLAM问题提供了一种解决思路. Martinez-Cantin等[10]提出将UKF应用于机器人位姿和地图的估计. 曲丽萍[11]等提出基于平方根的无迹卡尔曼滤波，在具有与UKF滤波相同精度的情况下，确保了计算的稳定性和协方差矩阵的半正定性. 在UKF-SLAM 的基础上，学者们提出迭代平方根的UKF-SLAM[12]、 AUKF-SLAM(Augmented UKF-SLAM)[13]等算法.除线性化误差和计算复杂度，基于卡尔曼滤波的SLAM算法中一个不容忽视的问题就是外部干扰. 在真实环境中，移动机器人轮胎打滑、遇到障碍物导致前进受阻、突发外力导致位置发生大的变化等外部干扰都会对SLAM产生较大的影响，因此降低其鲁棒性，甚至导致定位和地图创建的失败. 传统的卡尔曼滤波算法在受到外部干扰的情况下会导致误差急剧增加，但随着不断的观测来更新状态，误差会逐渐减少，所以算法不需要考虑外部干扰的问题. 但卡尔曼滤波算法应用于SLAM问题上，外部干扰在影响移动机器人定位的同时也会影响到环境地图. 环境地图和移动机器人定位相互影响，不会随着观测来减少误差，而是进一步累计误差. 传统UKF-SLAM算法并没有考虑外部干扰的影响，因此，当发生外部干扰情况的时候， UKF-SLAM算法会导致估计的不确定性远小于真实情况，从而降低了SLAM的精度和鲁棒性. 为了减少外部干扰对SLAM算法的影响，近几年各种不同的算法被提出. Ting[14]等通过对观测数据采用增量期望值最大的算法来检测外部干扰. Havangi[15]提出改进的模糊神经网络算法来动态调整先验知识的协方差. 然而该算法只调整观测误差的协方差矩阵R，同时也容易受到累计误差的影响. Choi[16]等提出基于协方差膨胀的移动均值(Shifted Mean Based Covariance Inflation Technique, SMCI)方法来降低外部干扰的影响. SMCI-SLAM算法通过方差膨胀来减少外部干扰的影响，但效果在下一个周期才能体现，同时没有区别对待不同类型的外部干扰，也容易受累计误差的影响.基于前期研究成果[17]，针对外部干扰会导致基于UKF-SLAM算法精度降低甚至发散的问题，提出了一种改进的UKF-SLAM算法. 算法主要特点在于:1) 继承抗外部干扰的EKF-SLAM算法，将其应用于UKF-SLAM算法，通过临近观测分析来检测外部干扰，不会受累计误差的影响，提高了外部干扰检测的精度.2)不仅处理干扰对控制输入的影响，也处理干扰对观测过程的影响. 将干扰分类处理，根据不同类型的外部干扰使用不同方式来膨胀系统状态方差，扩大其不确定性，使系统状态迅速收敛到真值.3)改进干扰检测算法，根据临近观测对比以及控制噪声和观测噪声协方差矩阵来判断是否发生小概率事件，以此确定SLAM过程是否存在外部干扰.1 SLAM问题描述SLAM问题可以描述为机器人在未知环境中从一个未知的位置开始移动，在不断的移动中根据控制信息和传感器观测进行自身定位，同时构建增量式地图. SLAM 的主要目标就是基于一组从开始到t时刻带有误差的控制数据u1:t和观测数据z1:t中获得对移动机器人位姿和地图信息最好的估计. SLAM问题可以描述为如下的后验概率.p(xi,Θ|z1:t,u1:t),(1)式中： xt是当前机器人的位姿；Θ是环境地图. xt可以通过上一时刻位姿xt-1和当前时刻控制数据ut的概率函数来表示，称之为运动模型.xt=g(xt-1,ut)+εt,(2)式中：εt为服从高斯分布的运动模型噪声. 通过传感器获得的观测数据同样可以被概率函数表示，称之为观测模型.zt=h(xt,Θ)+δt,(3)式中：δt为服从高斯分布的观测模型噪声.基于运动和观测模型， SLAM问题的后验概率可以使用贝叶斯滤波器迭代更新，式(1)改写成p(xt,Θ|z1:t,u1:t)=ηp(zt|xt,Θ,z1:t,u1:t),p(xt,Θ|z1:t-1,u1:t,x0).(4)卡尔曼滤波和粒子滤波都是典型的贝叶斯滤波. 卡尔曼滤波是将SLAM后验概率描述为均值为xt，协方差为Pt的多维高斯分布. 粒子滤波则是用有限的样本集合来代表SLAM的后验分布.2 UKF-SLAM算法EKF在迭代更新移动机器人状态时采用泰勒级数线性化系统模型，不可避免地引入了线性化模型误差，同时雅可比矩阵的更新也增大了EKF的计算量. 无迹卡尔曼滤波(Unscented Kalman Filter, UKF)是用2L+1(L为系统状态的维数)个能表征系统状态向量的Sigma点重构出系统统计特性. 通过非线性系统模型来实现机器人系统状态的更新，提高了估计的精度，同时该算法不需要推导系统的雅可比矩阵，节省了计算时间. 基于此， UKF被广泛应用于SLAM问题中[10, 18].UKF-SLAM初始化过程包括均值、协方差矩阵和Sigma点权值的设置.x0=xinit P0=Pinit,(5)(6)(7)λ=α2(L+κ)-L为尺度调节因子，常量α决定了Sigma点在均值附近的分布，通常将α设置为1×10-4≤α≤1，κ为次级尺度调节因子，一般设置为0. β是用来结合系统状态的先验分布，对于高斯分布，β取值为为第i个Sigma点均值的权值，为第i个Sigma点方差的权值.通过Sigma点计算、预测和观测更新三个步骤完成状态向量和协方差矩阵的迭代更新.1) Sigma点计算若t-1时刻系统状态均值和协方差为xt-1和Pt-1，首先增广系统状态向量得到如下均值和方差.(9)式中： Q和R分别代表控制噪声和观测噪声的协方差矩阵. 根据增广矩阵计算Sigma点.(10)尺度常量η计算如下(11)2) 预测根据上一时刻的Sigma点和运动模型预测t时刻的Sigma点.(12)(13)(14)3) 更新(15)(16)(17)(18)(19)Kt=Pxtxt(Pxtzt)-1,(20)(21)3 改进的UFK-SLAM算法3.1 算法设计思路在未知环境中探索的时候，外部干扰会导致误差突然增大，从而降低SLAM估计精度和鲁棒性. 在SLAM过程中，移动机器人会根据上一时刻的状态和控制输入来预测当前位姿和路标位置. 如果观测传感器观测的数据与预测的数据一致，系统状态的更新是可靠的；反之则是不可靠的，有很大可能性发生了外部干扰. 外部干扰主要分为以下两类情况：一类是移动机器人自身受到外部干扰，导致此类情况的主要原因有：① 外力的作用而导致移动机器人位姿发生较大的改变；② 移动机器人遇到障碍物或者地面潮湿等原因而导致车轮打滑使得从里程计读取的数据不能真实反映其运动状态；③ 惯性测量单元发送故障，读取的是错误数据.另一类情况是路标受到外部干扰，导致此类情况的主要原因有：① 由于外力的作用导致路标的自身位置发生了改变；② 观测传感器发生故障，读取的是错误数据. 针对外部干扰影响SLAM估计精度问题，本文提出一种改进的UKF-SLAM算法. 本文算法在UKF-SLAM算法基础上增加干扰检测和系统状态方差膨胀的过程. 临近观测的不一致性能判断出预测和观测数据是否一致，被用于外部干扰的检测. 当检测到存在外部干扰的时候，针对不同类型的外部干扰，通过膨胀系统状态协方差矩阵，扩大其不确定性，使系统状态迅速收敛到真值，提高算法的估计精度和鲁棒性.3.2 干扰检测和状态方差膨胀提出算法通过对比t-1时刻和t时刻的观测来检测移动机器人是否受到外部干扰. 假定t-1时刻移动机器人处于原点位置，前进的方向为X轴. t-1时刻机器人观测到的第j个路标信息为zt-1(mj)= [lt-1(mj), βt-1(mj)]. 以t-1时刻移动机器人位置为原点，第j个坐标的位置为(23)式中：和是第j个路标的X和Y轴坐标信息. 根据控制信息、运动模型和t-1时刻移动机器人原点位置， t时刻移动机器人预测位姿为(24)式中：ΔT为采样周期； vt为车辆后轴中心处运动速度；αt为车辆的转向角； L 为两轴的轴距. 通过平移和旋转操作，可以获得第j个路标以t时刻机器人为原点下的预测位置(25)将和转换为极坐标就是移动机器人对于第j个路标在t时刻的预测观测.(26)最后通过临近两次观测不一致性来确定移动机器人或者路标是否受到外部干扰. 1) 移动机器人受到外部干扰根据对路标观测的距离误差和角度误差分别来判断移动机器人是否受到外部干扰.是预测值与观测值之间距离差值的平方，将其标准化：(27)式中：Sm是观测函数关于路标位置的雅可比矩阵. Dlt(mj)～N(0,1)服从正态分布. 当该值大于2.3即小概率事件发生的时候，表示由于外部干扰的影响导致对路标的观测存在较大误差的可能性较大. 根据观测路标数量、控制和观测误差协方差矩阵设定阀值，当存在较大观测误差的路标数量超过阀值，就认定移动机器人受到外部干扰.转向角度误差是导致SLAM算法估计不一致性的重要因素，该误差变大将大大降低SLAM的估计精度.是预测值与观测值之间角度差值的平方，将其标准化(28)Dβt(mj)～N(0,1)服从正态分布. 当对多个路标观测Dβt(mj)超过指定阀值并且角度偏移方向趋于一致的时候就认定移动机器人受到外部干扰.当判断移动机器人受到外部干扰的时候，下一步的工作就是修复此类干扰. 本文算法对移动机器人位姿状态的协方差矩阵进行膨胀，以此来加大机器人的不确定性，让系统状态更倾向于观测的结果，从而在更新过程中使机器人更接近于真实的位置.(29)式中： avg(diffl)和avg(diffβ)代表路标预测值和观测值在距离和角度上的平均偏差值； Gut是运动模型关于控制输入的雅可比矩阵.2) 路标受到外部干扰当只有单一路标预测值与观测值之间的距离差值超过阀值，就认定该路标由于受到外部干扰而导致位置发生了改变. 假定第j个路标受到外部干扰，通过对该路标的协方差矩阵Pm,j进行膨胀,以此来加大该路标估计的不确定性，让系统状态更倾向于移动机器人的位姿和其他路标观测的结果，从而使得对系统状态的更新更接近于真实值.(30)式中： diffl,j和diffβ,j代表第j个路标预测值和观测值在距离和角度上的偏差；Ht是观测模型关于状态向量的雅可比矩阵.3.3 算法流程本文算法改进UKF-SLAM，增加了干扰检测的过程，当检测到系统中存在外部干扰时，通过对系统状态方差的膨胀，加大移动机器人或路标估计的不确定性，提高了SLAM估计的精度. 算法1描述t时刻干扰检测和状态方差膨胀的流程.算法1 干扰检测和状态方差膨胀输入：t-1时刻协方差矩阵Pt-1， t-1时刻观测zt-1， t时刻观测zt步骤：∥如果临近观测相同路标数量小于3，不进行外部干扰检测if (sameObservedLandmark(zt-1, zt)<3)return;end if∥根据临近两次观测检测是否存在外部干扰disturanceType=checkDisturance(zt-1, zt);if disturanceType==ROBOTDISTURANCE∥根据式(28)膨胀协方差矩阵else if disturanceType==LANDMARKDISTRUANCE∥根据式(29)膨胀协方差矩阵end if输出：t时刻预测的协方差矩阵本文算法在UFK-SLAM数据关联和系统状态更新流程之间增加了干扰检测和状态方差膨胀的过程. 由于是在系统状态更新之前引入外部干扰检测和状态方差膨胀，所以本文算法能在干扰发生的周期内做出快速响应. 图 1 描述了t时刻改进UKF-SLAM算法更新系统状态的流程.图 1 改进UKF-SLAM算法流程图Fig.1 The flowchart of the improved UKF-SLAM algorithm4 仿真实验结果及分析为了测试算法性能，设计了UKF-SLAM[10]、 SMCI-SLAM[16]和改进UKF-SLAM算法的仿真对比实验. 移动机器人t时刻的运动和观测模型采用式(31)和(32). 给定控制输入ut=[vt, αt]， vt是车辆后轴中心处运动速度，αt是车辆的转向角. 移动机器人运动模型可表示为(31)式中：ΔT为采样周期；ε为控制噪声； L为两轴的轴距. SLAM问题假定控制噪声服从高斯分布.观测数据由路标的距离lt(mj)和方向角βt(mj)组成. t时刻移动机器人观测方程为(32)其中，方向角βt的取值范围为处于车辆正前方路标的方向角为0 rad.仿真环境基于Matlab 2013平台，移动机器人的运动速度是3 m/s，最大转向角是速度噪声是0.1 m/s, 转向转角误差为机器人配置了π rad，最远观测距离为30 m的激光雷达. 观测的距离误差为0.1 m，角度误差为 rad.4.1 移动机器人受到外部干扰设计一个大小为250 m×200 m的包含75个路标的仿真环境，如图 2(a)所示，移动机器人的初始位置为(0,0).图 2 三种算法在机器人受到外部干扰下的实验结果Fig.2 Comparison between the three algorithms with outlier disturbances on robots移动机器人在此环境运动过程中，每100个控制周期会随机产生3～5次外部干扰. 由于外部干扰的存在导致移动机器人实际的位姿改变与控制输入相差甚远，从而影响机器人定位和地图创建的精度. 在此环境中采用UKF-SLAM、 SMCI-SLAM 和改进UKF-SLAM算法对移动机器人的路径和环境中的路标进行估计，三种算法均假定数据关联已知. 仿真实验结果如图2(b)～图2(d)，图中实线表示移动机器人的运行轨迹，虚线表示各个算法预估的路径. 加号表示路标的真实位置，圆圈表示各个算法估计的路标位置.从图 2 中可以看出，虽然三种算法与机器人真实的路径都存在一定的误差，但是改进UKF-SLAM算法无论在机器人路径估计还是路标位置估计上的误差都要小于UKF-SLAM和SMCI-SLAM算法，即改进UKF-SLAM算法的估计更接近真实的状态.由于在仿真环境中，运动和观测噪声是随机的，每次实验的结果都是不一样的，所以不能仅用某一次的实验结果来判断算法的性能. 均方根误差(root-mean squared error, RMSE)用来衡量观测值和真实值之间的偏差，能很好地反映出算法的精确度. 以RMSE为标准，对UKF-SLAM、 SMCI-SLAM和改进UKF-SLAM算法进行比较. 使用图2(a)所示仿真环境，在机器人运动过程中，随机产生外部干扰. 为了更详细地评估三种算法的性能，实验执行了50次，结果如表 1 所示. RMSE_P表示机器人位置估计的均方根误差， RMSE_L表示路标位置估计的均方根误差.表 1 三种算法的50次仿真实验对比Tab.1 The comparison of 50 experimental results between the three algorithms算法RMSE_P/mRMSE_L/m执行时间/minUKF-SLAM算法6.196.076.43SMCI-SLAM4.374.055.49改进UKF-SLAM算法2.862.606.86改进UKF-SLAM算法比UKF-SLAM在移动机器人位置和路标估计上的RMSE分别降低了53.80%和57.16%，比SMCI-SLAM算法在移动机器人位置和路标位置估计的RMSE分别下降了31%和35%. 实验证明，在移动机器人受到外部干扰的环境中，通过干扰检测和状态方差膨胀可以改进SLAM估计的精度. 由于干扰检测只对比了相邻两次观测的路标，所耗费的时间有限，故改进UKF-SLAM算法与UKF-SLAM和SMCI-SLAM算法执行时间上接近.在50次仿真实验中，移动机器人每次运动大约190 s. 图 3 所示为UKF-SLAM、SMCI-SLAM和改进UKF-SLAM算法基于时间的移动机器人位姿估计均方根误差曲线图. 图中包含了四幅子图，分别是基于X轴、 Y轴、方向角和位置的均方根误差. 从图3中可以看出，改进UKF-SLAM比其他两种算法在整个实验过程中都更接近移动机器人的真实位置，证明干扰检测和状态方差膨胀可以有效提高移动机器人位姿估计精度.图 3 三种算法在机器人位姿上的均方根误差Fig.3 The RMSE comparison in position among the three algorithms图 4 所示为路标位置的均方根误差曲线图. 从图中可以看出，改进UKF-SLAM算法比其他两种算法对于路标位置的估计误差要小.图 4 三种算法在路标位置上的均方根误差Fig.4 The RMSE comparison in landmark position between the three algorithms4.2 路标受到外部干扰采用与前面实验相同的仿真环境，在机器人移动过程中坐标为(12,-3), (125,105)和(-20,140)的3个路标由于受到外部干扰的影响，位置发生了改变. 在路标受到外部干扰的环境下使用UKF-SLAM、 SMCI-SLAM和改进UKF-SLAM算法对移动机器人的路径和环境中的路标进行估计. 三种算法均假定数据关联已知，仿真实验结果如图 5(b)～图5(d)所示.图 5 三种算法在路标受到外部干扰下的实验结果Fig.5 Comparison between the three algorithms with outlier disturbances on landmarks在UKF-SLAM算法中，某个路标位置的改变不仅影响地图估计，也影响了移动机器人定位的精度. 由于采用固定的噪声模型，使得对位置已改变路标的过分信任，导致移动机器人位姿和其他路标位置向已改变位置路标偏移. 在UKF-SLAM算法中误差是累计的，最终会使得无论在地图估计上的误差还是机器人定位估计上的误差都会变大. SMCI-SLAM算法和改进UKF-SLAM算法通过对系统状态的膨胀降低了对位置改变路标的信任，提高了SLAM的估计精度. 通过系统状态膨胀，虽然可能会降低被改变路标位置估计的精度，如图5(d)改进UKF-SLAM算法对于坐标为(12,-3)的路标估计精度不如UKF-SLAM，但是如果不膨胀系统状态，会导致移动机器人定位和地图中其他路标位置估计精度的降低，如图5(b)中，由于该路标位置的改变，导致移动机器人的位置和其他路标位置都发生了偏移，由于误差是累计的，从而导致后面对系统状态估计的误差越来越大. SMCI-SLAM算法是在路标受到外部干扰影响的下一个周期才使用膨胀后的协方差更新地图和机器人位置，同时也受累计误差的影响. 改进UKF-SLAM算法在路标受到外部影响的周期内就使用膨胀后的协方差更新系统状态，能够快速地对外部干扰做出反应，同时不受累计误差的影响，也对外部干扰做出了分类，所以更能反映真实的环境. 从图中也可以看出，改进UKF-SLAM估计的精度要优于SMCI-SLAM和UKF-SLAM算法.由于运动和观测噪声的随机性导致每次实验的结果不一样，这里仍然以RMSE为标准，对UKF-SLAM、 SMCI-SLAM和改进UKF-SLAM算法进行比较. 环境地图使用图5(a)的仿真环境，在移动机器人运动过程中，会随机产生外部干扰导致3～5个路标位置发生改变. 实验执行了50次，结果如表 2 所示，结果表明在路标受到外部干扰的环境中，通过干扰检测和状态方差膨胀可以改进SLAM估计的精度.表 2 三种算法的50次仿真实验对比Tab.2 The comparison of 50 experimentalresults between the three algorithms算法RMSE_P/mRMSE_L/m执行时间/minUKF-SLAM算法5.657.346.43SMCI-SLAM4.726.305.49改进UKF-SLAM算法2.742.896.865 结束语对外部干扰影响SLAM估计精度的问题，提出了改进的UKF-SLAM算法. 所提算法在系统状态更新之前引入外部干扰检测和状态方差膨胀，能在干扰发生的周期内快速做出响应. 临近观测的不一致性用于检测外部干扰不受累计误差的影响，提高了检测的精度. 外部干扰分为移动机器人受到的外部干扰和路标受到的外部干扰，对它们使用不同方式膨胀系统状态协方差可以获取更精确的系统状态估计. 仿真实验结果表明，改进UKF-SLAM算法比UKF-SLAM和SMCI-SLAM算法在系统发生外部干扰的情况下对系统状态的估计更加精确，同时增加的系统开销也很小.参考文献：【相关文献】[1]林亨, 周源, 刘宇飞. 技术热点、前沿识别支持的2035技术清单调整方法——以机器人技术为例[J]. 中国工程科学, 2017, 19(1): 124-132.Lin Heng, Zhou Yuan, Liu Yufei. Technology hot spots and frontier identification supportto 2035: the technology list adjustment method, using robot technology as an example[J]. Engineering Sciences, 2017, 19(1)： 124-132. (in Chinese)[2]Fernández-Madrigal J, Claraco J. Simultaneous localization and mapping for mobile robots： introduction and methods[M]. Hershey： IGI Global, 2013.[3]谭民, 王硕. 机器人技术研究进展[J]. 自动化学报, 2013, 39(7)： 963-972.Tan Min, Wang Shuo. Research progress on robotics[J]. ACTA Automatica Sinica, 2013,39(7)： 963-972. (in Chinese)[4]张琦. 移动机器人的路径规划与定位技术研究[D]. 哈尔滨：哈尔滨工业大学, 2014.[6]陈晨. 基于Sigma点滤波的移动机器人同时定位与地图创建算法的研究[D]. 北京：北京交通大学, 2012.[8]Barrau A, Bonnabel S. Invariant filtering for pose EKF-SLAM aided by an IMU[C]∥Proc. of the 54th IEEE Conference on Decision and Control, 2015： 2133-2138.[9]周武. 面向智能移动机器人的同时定位与地图创建研究[D]. 南京：南京理工大学, 2009.[10]Martinez-Cantin R, Castellanos J. Unscented slam for large scale outdoor environments[C]∥Proc. of the 2005 International Conference on Intelligent Robots and Systems, 2005： 328-333.[11]Qu L, He S, Qu Y. An SLAM algorithm based on improved UKF[C]∥Proc. of the 24th Chinese Control and Decision Conference, 2012： 4154-4157.[12]赵琳, 闫鑫, 郝勇. 基于迭代平方根UKF的SLAM算法[J]. 弹箭与制导学报, 2011, 31(1)： 157-160.Zhao Lin, Yan Xin, Hao Yong. SLAM algorithm based on iterated square unscented Kalman filter[J]. Journal of Projectiles, Rockets, Missiles and Guidance, 2011, 31(1)： 157-160. (in Chinese)[13]Maeyama S, Takahashi Y, Watanabe K. A solution to SLAM problems by simultaneous estimation of kinematic parameters including sensor mounting offset with an augmented UKF[J]. Advanced Robotics, 2015, 29(17)： 1137-1149.[14]Ting J, Theodorou E, Schaal S. Learning an outlier-robust Kalman filter[C]∥Proc. of the 2007 European Conference on Machine Learning, 2007： 748-756.[15]Havangi R, Nekoui M A, Teshnehlab M. An improved FastSLAM framework using soft computing[J]. Turkish Journal of Electrical Engineering and Computer Sciences, 2012,20(1)： 25-46.[16]Choi W, Oh S. Robust EKF-SLAM method against disturbance using the shifted mean based covariance inflation technique[C]∥Proc. of the 2011 IEEE International Conferenc e on Robotics and Automation, 2011： 4054-4059.[17]吕太之. 一种新的抗外部干扰EKF-SLAM算法[J]. 计算机工程, 2012, 38(21)： 1-4.Lü Taizhi. A Novel EKF-SLAM algorithm against outlier disturbance[J]. Computer Engineering, 2012, 38(21)： 1-4. (in Chinese)[18]Havangi R. Robust SLAM：SLAM base on H∞ square root unscented Kalman filter[J]. Nonlinear Dynamics, 2016, 83(1-2)： 767-779.。

rls算法原理RLS（Recursively Least Square）算法，又称递推最小二乘法，是广泛应用于自适应信号处理、滤波、预测和识别等领域的一种算法。

RLS算法的核心思想是利用已知的数据来递推估计某一未知参数的值。

在实际应用中，经常需要从一系列观测数据中，准确地估计出某个未知参数或者变量，例如找到某个物体的位置、估计某个信号的频率和幅度等等。

此时，常常需要利用数学模型推导出最优的估计方法，而RLS算法就是基于最小二乘法推导出来的一种递推算法，可以有效地解决这类问题。

具体地说，RLS算法的实现主要包括三个步骤：1.建立估计模型估计模型是指在已知的观测数据和所要估计的未知参数之间建立的联系模型。

对于一个n维的模型来说，有如下形式：$$\boldsymbol{y}(n)=\boldsymbol{x}^T(n)\cdot\boldsymbol{w}(n)+\boldsymbol{v}(n)$$其中，$\boldsymbol{y}(n)$是观测数据，$\boldsymbol{x}(n)$是一组特征变量（也称为自变量），$\boldsymbol{w}(n)$是待估参数，$\boldsymbol{v}(n)$是噪声项。

该模型的估计目标是通过已知的$\boldsymbol{y}(n)$和$\boldsymbol{x}(n)$来递推估计$\boldsymbol{w}(n)$的值。

2.递推计算参数采用递推计算的方式，计算出每一时刻的参数估计值。

具体地说，假设当前时刻为$n$，则根据已知的先验信息，我们可以得到关于$\boldsymbol{w}(n)$的先验分布$I(n)$，同时我们也已经获得了观测数据$\boldsymbol{y}(n)$和自变量$\boldsymbol{x}(n)$。

因此，我们可以根据贝叶斯理论，推导出后验分布$P(\boldsymbol{w}(n)|\boldsymbol{y}(n),\boldsymbol{x}(n),I(n))$以及最小二乘估计公式。

mimo信道估计算法MIMO（Multiple-Input（Multiple-Output）系统是指在发送端和接收端都有多个天线的通信系统。

MIMO信道估计是指在这样的系统中对信道进行估计，以便在接收端恢复传输的数据。

信道估计在MIMO 系统中至关重要，因为它可以帮助系统更准确地了解信道状况，从而提高通信的可靠性和性能。

以下是一些常见的MIMO信道估计算法：1.（最小均方误差 MMSE）估计•MMSE是一种常用的线性估计算法，通过最小化均方误差的方法来估计信道。

•它考虑了信道噪声和信号的相关性，可以在噪声存在的情况下有效地估计信道。

2.（最大似然估计 MLE）•MLE是一种基于概率的估计方法，假设接收到的信号是从某个已知概率分布中抽取的。

•它寻找最有可能产生接收信号的信道参数，通常在理想情况下提供较好的性能。

3.（奇异值分解 SVD）•SVD是一种将MIMO信道矩阵分解成几个较小矩阵的方法，其中包括信道矩阵的正交特征向量。

•通过SVD，可以在不同的信道传输路径上进行分解和估计，提高了信道估计的准确性。

4.（协方差矩阵估计•该方法尝试估计接收信号的协方差矩阵，从而推断信道状况。

•通过协方差矩阵的估计，可以获取信道的统计特性，对信号进行优化处理。

5.（基于导频的估计•这种方法利用发送端发送的已知导频信号来估计信道状况。

•接收端根据接收到的导频信号与已知的导频信号进行比较，推断信道特性。

6.（贝叶斯估计•贝叶斯估计利用贝叶斯定理，结合先验信息和观测数据来进行信道估计。

•它可以提供对信道参数的概率分布估计，更全面地描述了不确定性。

这些算法都有各自的优劣和适用场景，选择合适的算法取决于通信系统的特性、噪声条件、计算复杂度和精确度要求等因素。

在实际应用中，通常需要结合不同的算法和技术来进行MIMO信道估计，以获得更好的性能和可靠性。