CN117879540A - 基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法 - Google Patents

Abstract

本发明公开了一种基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法，属于信号处理领域，其方法为在常规卡尔曼滤波方程的基础上，增设系统协方差矩阵修正参数，并优选增设条件阈值和系统特性修正系数；本发明的方法在滤波过程中能够根据当前测量值的情况而自适应调整数字滤波器卡尔曼增益，达到保证传感器数据具有较高滤波精度的基础上，满足磁罗盘实时解算输出姿态角的功能需求，有效降低传统滤波时滞的效果。

Description

基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法

技术领域

本发明涉及信号处理领域，尤其涉及一种基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法。

背景技术

卡尔曼滤波是一种测量过程中重要的估计方法，能有助于得到更精确的估计推算，从而改善数据采集测量系统的性能。一维卡尔曼滤波利用观测和测量来确定未知变量的过程，它假定测量变量呈稳定的线性方式被观测，同时观测和测量之间存在一个可描述的过程噪声存在。其中心思想是利用当前最新的观测值和误差估计的状态值的结合，不断估计即将发生的未知状态，从而得到接近真实变量的估计值。

一维卡尔曼滤波算法采用递归的方法解决线性滤波问题，只需知道当前测量值和前一个采样周期的估计值就能进行状态估计，需要的存储空间小，且每一步的计算量小。常规卡尔曼的卡尔曼滤波一共有5个计算方程，整个计算过程由5步进行递归实现：

第1步 预测系统状态方程：

由系统状态变量k-1时刻的最优值和系统输入计算出k时刻的系统预测值，方程如下：

=/>+/> （8），

式中，是利用k-1时刻预测的状态结果；/>是k-1时刻最优值；/>是作用在/>状态下的变换矩阵，也是算法对状态变量进行预测的依据；/>是作用在控制量的变换矩阵；/>是当前状态的控制增益，在一般情况下，该值为0，没有控制增益；

当测量值是一维数据时，是恒定为1的，此外，一维数据下（/>=0时），系统预测值就等于k-1时刻的最优值，即上式可变换为：

=/> （9），

第2 步 预测协方差方程：

系统根据k-1时刻的系统协方差预测k时刻系统协方差，方程如下：

=/>+/> （10），

式中，是k时刻的系统协方差矩阵；/>是k-1时刻的系统协方差矩阵；是系统过程噪声的协方差。其中系统协方差矩阵只要一开始确定了/>，在后续的递归过程中系统的协方差就会递推计算得到；

同样，当系统是一维数据时，是恒定为1的，上式可变换为：

=/>+/> （11），

第3步 计算卡尔曼增益方程：

根据k时刻的协方差矩阵的预测值，计算卡尔曼增益，方程如下：

=/> （12），

式中，是卡尔曼增益；/>为对象的预测矩阵；/>为对象测量噪声的协方差矩阵，它为一个固定数值，作为已知条件输入滤波器模型，此外，该值过大过小会直接影响滤波效果，一般通过实验测得，其值越小收敛越快；

当系统是一维数据时是一维矩阵，此时，/>值为1，上式可变换为：

=/> （13），

第4步 求解系统最优值方程：

根据状态变量的预测值和系统测量值计算出k时刻状态变量的最优值，其方程如下：

=/>+/>(/>-/>) （14），

式中，为k时刻状态变量最优估计值；/>是对象k时刻测量值；

同样，当系统是一维数据时是一维矩阵，此时，/>值为1，上式可变换为：

=/>+/>(/>-/>) （15），

第5步 更新协方差方程：

求解k时刻的协方差矩阵，为k+1时刻的卡尔曼输出值做准备，实现递归计算，其方程如下：

=(I-/>)/> （16），

式中，是k时刻协方差矩阵，I为单位矩阵；

同样，当系统是一维数据时是一维矩阵，此时，/>值为1，上式可变换为：

=(1-/>)/> （17），

通过，上述5个步骤，可以实现对测量值进行递归计算，并实现很好的滤波效果。最终整理得到常规卡尔曼一维卡尔曼滤波算法的5个计算公式如下：

（18）。

在常规卡尔曼一维数据滤波算法运行时，只需给定一组初始滤波参数（包括、/>、/>、/>、/>，其中/>不能为0），不断采集输入当前测量值/>，上述五组公式就能正常递归运行起来，实现不断迭代优化，最终达到数据估算滤波效果。当测量值/>一直处于较稳定输入状态（传感器静止或者无突发干扰输出状态），由于/>和/>始终是一个固定值，故公式（13）所得到的/>会不断缩小且逐渐稳定，从而导致公式（15）中输出结果/>更信赖K-1时刻的估计值/>，这里等效于一维数字低通滤波算法效果。然而，当系统测量值/>突变时，虽然此时的/>有较大改变，由于此时/>很小，导致公式（15）输出结果/>不能有效跟踪上系统测量值/>突然改变。如图1所示，图1中上图为常规卡尔曼卡尔曼滤波结果，下图为实时的/>变化情况。当输入信号发生突然变化时，常规卡尔曼卡尔曼会发生较明显时滞现象，此时的/>基本没有变化。

在对磁罗盘加速度传感器数据和磁传感器数据数字滤波时，通常在磁罗盘处于长时间静止的数据采集状态时，由于数据变化量不大，使用常规一维卡尔曼滤波算法对传感器采集数据进行数字滤波，通过优化初始化校准参数后能得到准确的估计值。可是，一旦罗盘姿态改变或者周围磁场环境发生改变，此时伴随着加速度和磁传感器数据的较大突变，常规卡尔曼的一维卡尔曼滤波会发生严重的滤波时滞，不能很好跟随测量值的实时变化，导致滤波后数据严重失真。由于一维卡尔曼滤波是一种递归算法，其滤波参数在滤波过程中会实时更新，故这种滤波时滞不可能通过调整初始化的卡尔曼滤波参数来避免。

因此，本领域亟需一种新的方法，以便于在滤波过程中根据当前测量值的情况而自适应调整滤波参数，以达到保证较高精度滤波精度的基础上，满足罗盘解算姿态角时传感器数据实时更新的需求，降低常规卡尔曼滤波时滞。

发明内容

本发明的目的就在于提供一种基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法，以解决上述问题。

为了实现上述目的，本发明采用的技术方案是这样的：一种基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法，用于对磁罗盘加速度传感器数据和磁传感器数据进行数字滤波，包括下述步骤：

（1）采用如下式的方程预测系统状态：

=/> （1），

其中，是利用k-1时刻预测的磁罗盘加速度传感器和磁传感器-状态结果，是k-1时刻磁罗盘加速度传感器和磁传感器输出最优值；

（2）采用如下式的方程预测数字滤波器系统协方差：

=/>+/> （2），

其中，是k时刻的数字滤波器系统协方差矩阵，/>是k-1时刻的数字滤波器系统协方差矩阵；/>是数字滤波器系统过程噪声的协方差；

（3）采用如下式的方程计算数字滤波器卡尔曼增益：

=/> （3），

其中，是数字滤波器卡尔曼增益，/>为对象测量噪声的协方差矩阵；

（4）采用如下式的方程求解系统最优值：

=/>+/>(/>-/>) （4），

其中，为k时刻磁罗盘加速度传感器和磁传感器输出最优估计值，/>是对象k时刻磁罗盘加速度传感器和磁传感器测量值；

（5）采用如下式的方程更新协方差：

=/>(I-/>)/> （5），

是k时刻数字滤波器协方差矩阵，I为单位矩阵，/>是数字滤波器系统协方差矩阵修正参数。

本方法原理上是对一种数学方法的改进，所以它适用于所有的数据采集滤波，应用于数字罗盘就是对罗盘上的两种传感器（磁罗盘加速度传感器和磁传感器）。这两种传感器滤波计算过程是一样的，差异在于具体滤波参数的不同，包括、/>、/>、/>、/>，其中/>不能为0。

本发明在常规卡尔曼卡尔曼滤波方法的基础上，增设了系统协方差矩阵修正参数，从而得到改进后的自适应一维数据卡尔曼实时滤波算法的计算公式如下：

（7），

作为优选的技术方案，还对系统协方差矩阵修正参数进行调整。

作为进一步优选的技术方案，所述调整方法为：

当≤ΔH时，

=1 （6-a），

否则，

（6-b），

其中，是系统k时刻磁罗盘加速度传感器和磁传感器测量值，/>是k-i时刻的磁罗盘加速度传感器和磁传感器测量值，/>是条件阈值，/>为系统特性修正系数，其值大小与被测信号幅值范围与信号特性相关，是一个测试设定值。

为了达到自适应的目的，本发明增设了条件阈值和系统特性修正系数/>。

本发明的方法需要系统实时检测系统k时刻测量值和k-i时刻的测量值/>，将它们作差后的绝对值/>与条件阈值/>进行对比：当小于等于阈值/>时，则认为系统处于稳定状态，无需修正系统协方差矩阵，则/>；而当偏差大于/>时，则认为系统输入发生较大改变，此时/>的调整除了与系统特性修正系数/>相关外，还与存在较大关系。当/>值较大时，/>改变就越大，调整能力也就越强；反之就更多的使用系统特性修正系数/>来调节。

与现有技术相比，本发明的优点在于：本发明的方法在滤波过程中能够根据当前测量值的情况而自适应调整滤波参数，达到保证较高精度滤波精度的基础上，满足实时测试需求，降低传统滤波时滞的效果。

附图说明

图1为常规卡尔曼卡尔曼滤波效果图；

图2为本发明实施例1与对比例1效果对比图；

图3为本发明实施例1的自适应算法滤波效果图；

图4为本发明实施例2与对比例2效果对比图；

图5为本发明实施例3与对比例3效果对比图。

具体实施方式

下面将结合附图对本发明作进一步说明。

实施例1：

将磁罗盘放置在水平面上，利用一铁棒靠近磁罗盘，由于铁棒磁场的干扰，将实时采集得到一组突变的罗盘磁传感器原始数据。将此原始数据分别采用常规卡尔曼算法（采用公式（18））和改进卡尔曼滤波算法（采用公式（6-a、6-b））进行处理，并将两者处理后磁传感器最优估计值和卡尔曼滤波增益进行对比。结果如图1和图3所示。在图1中，铁棒放置前期/>迅速调整，此时滤波效果显著，数据噪声得到了明显改善。当铁棒靠近时磁场发生偏移，然而常规卡尔曼滤波数据发生了明显滞后，观察此时的增益/>基本没有变化，数据发生严重失真，导致罗盘数据失效。等待大概2秒后，数据便逐渐稳定下来，滤波器再一次回复滤波功能。图3是使用本发明所述的改进卡尔曼滤波方法（即图3中的“自适应卡尔曼滤波”），滤波数据在铁棒靠近瞬间，就改变了增益/>的值，而且数据突变越大增益/>的值改变越大，即具有自适应调节大小的功能。由图中可见，经过改进方法滤波输出的数据在保证滤波效果的同时，能很快跟随上数据突变具有很强的实时性。图2是将两种方法放在一起对比，由图可见本发明的新方法（实施例1）在原先滤波方法上，极大地提高了滤波的实时性，满足了罗盘使用时对传感器数据快时延、低噪的需求，解决了磁罗盘数据抖动和时滞的问题。

结果如图2和图3所示，图2中可见本实施例的自适应实时滤波算法，解决了传统算法时滞的问题，保证了信号的滤波效率；与图1对比可知，图3中新算法在输入信号发生改变后，能根据输入信号改变的强烈情况，作出及时快速准确的改变，从而有效的保证了算法的实时性。

实施例2：

改变罗盘在空间内姿态，实时采集得到一组突变的磁罗盘加速度传感器原始数据。将此原始数据分别采用常规卡尔曼算法（采用如前式（18）的计算公式）和本发明改进卡尔曼滤波算法（采用如前式（6-a、6-b）的计算公式）进行处理，得到实验对比结果图4所示，本发明改进后的卡尔曼滤波能更好的跟随加速度数据的突变，并且具备较好的数据滤波效果。而常规的卡尔曼滤波只能在数据稳定后具有滤波效果，一旦数据突变就会发生跟随不及时，严重影响磁罗盘姿态解算功能。

实施例3：

改变磁罗盘空间姿态，实时采集得到一组突变的磁罗盘磁传感器原始数据，由于空间姿态改变，磁传感器数据也会发生较大变化。将此原始数据分别采用常规卡尔曼算法（采用如前式（18）的计算公式）和本发明改进卡尔曼滤波算法（采用如前式（6-a、6-b）的计算公式）进行处理，得到实验对比结果如图5所示。可见，使用本发明改进后的卡尔曼滤波不仅能保证数据滤波效果，还能跟随上磁传感器数据变化，满足磁罗盘姿态解算对数据源性能的需求。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于改进卡尔曼滤波的磁罗盘传感器自适应信号滤波方法，用于对磁罗盘加速度传感器数据和磁传感器数据进行数字滤波，其特征在于，包括下述步骤：

（1）采用如下式的方程预测系统状态：

=/> （1），

其中，是利用k-1时刻预测的磁罗盘加速度传感器和磁传感器-状态结果，是k-1时刻磁罗盘加速度传感器和磁传感器输出最优值；

（2）采用如下式的方程预测数字滤波器系统协方差：

=/>+/> （2），

其中，是k时刻的数字滤波器系统协方差矩阵，/>是k-1时刻的数字滤波器系统协方差矩阵；/>是数字滤波器系统过程噪声的协方差；

（3）采用如下式的方程计算数字滤波器卡尔曼增益：

=/> （3），

其中，是数字滤波器卡尔曼增益，/>为对象测量噪声的协方差矩阵；

（4）采用如下式的方程求解系统最优值：

=/>+/>(/>-/>) （4），

其中，为k时刻磁罗盘加速度传感器和磁传感器输出最优估计值，/>是对象k时刻磁罗盘加速度传感器和磁传感器测量值；

（5）采用如下式的方程更新协方差：

=/>(I-/>)/> （5），

是k时刻数字滤波器协方差矩阵，I为单位矩阵，/>是数字滤波器系统协方差矩阵修正参数。

2.根据权利要求1所述的方法，其特征在于，还对数字滤波器系统协方差矩阵修正参数进行调整。

3.根据权利要求2所述的方法，其特征在于，所述调整方法为：

当≤ΔH时，

=1 （6-a），

否则，

（6-b），

其中，是系统k时刻磁罗盘加速度传感器和磁传感器测量值，/>是k-i时刻的磁罗盘加速度传感器和磁传感器测量值，/>是条件阈值，/>为系统特性修正系数，其值大小与被测信号幅值范围与信号特性相关，是一个测试设定值。