组合导航(七):卡尔曼滤波 |
您所在的位置:网站首页 › 滤波模式导航 › 组合导航(七):卡尔曼滤波 |
Kalman滤波1 离散卡尔曼滤波2 卡尔曼滤波的流程2.1 预测与时间更新2.2 测量更新与校正
3 卡尔曼滤波 算法步骤4 非线性卡尔曼滤波4.1 线性化kalman滤波4.2 扩展kalman滤波
5 卡尔曼滤波发散控制5.1 KF过程模型中添加虚拟噪声5.2 Schmidt Epsilon方法5.3 有限记忆滤波5.4 渐消记忆滤波
6 卡尔曼滤波例子6.1 简单导航系统6.2 零速度更新6.2.1 通过ZUPT提高测量精度
6.3 坐标更新6.3.1 通过CUPT提高测量精度
Kalman滤波
导航系统的精度受到 惯性传感器初始化以及算法的误差影响。低成本MEMS传感器由于严重的随机误差,INS输出可能迅速漂移。因此低精度的IMU基本上不能作为导航独立传感器进行使用。 在之前的章节中曾叙述过,对于水平面的速度与姿态角而言,若没有对速度进行持续的优化以消除速度误差,在长时间运动情况下会导致Vn和Ve值产生巨大的误差。同时,姿态角的误差,也需要依靠这些速度值来调节 ——【舒勒效应】。因此,建立精确的速度分量模型是十分重要的(可消除pitch、roll角的误差)。 几类典型的测量更新方式: 1)位置 或坐标更新——“CUPT” 2)速度 或零速度更新——“ZUPT” 3)姿态更新 通过外部测量进行INS更新的方法有很多种:包括卡尔曼滤波(KF)、粒子滤波(PF)和人工智能(AI)。 通常,将惯性导航系统与GPS辅助系统相结合,利用卡尔曼滤波对惯性传感器误差进行估计和补偿。总的来说,卡尔曼滤波器是一种从被噪声污染的测量值中最优估计系统状态误差的算法。这是一个顺序递归算法,提供了一个最佳的误差最小均方差估计。 卡尔曼滤波的优秀之处在于,它使用了所有可用的测量数据,不论它们的精度高低,都可以通过对测量值增加合适的权重来估计当前系统状态。 在惯性导航应用中,卡尔曼滤波用于互补配置,将具有不同噪声特性的同一信号的冗余测量值结合起来,最小化误差。惯导系统能提供良好的高频信息,但由于积分运算,惯导系统的误差随时间不断增大,使得加速度计和陀螺仪的偏置误差在输出端积累。惯性传感器的偏置误差通常出现在传感器输出的低频部分,称为长期误差。另一方面,许多其他导航系统(如GPS)具有良好的低频特性,但容易产生高频噪声。因此,利用KF可以从外部源的准确低频数据中获益,从而限制INS的长期误差。有许多外源可以为INS提供可靠的帮助,包括雷达、GPS、车轮传感器、激光测距和存储的图像数据库。 在下文中将主要考虑全球定位系统作为外部源(由于GPS的准确性,全球可用性和低成本)。 1 离散卡尔曼滤波卡尔曼滤波要求系统为线性的系统,离散时间的线性系统可描述为: 系统的离散时间线性测量方程为: 1.状态方程和测量方程均可用线性模型表示; 2.系统噪声Wk以及测量噪声 为不相关的零均值高斯白噪声: 3.系统初始状态向量xo是一个与过程和测量噪声都不相关的随机向量,因此: KF是一种递归算法,通过状态反馈来估计系统的状态。 如图所示,卡尔曼滤波分为两个环节:(1)预测 /状态更新; (2)校正 /测量更新。 在预测阶段,将系统状态及其协方差从k -1时刻传播到k时刻;然后在校正阶段,使用测量值更新先前的估计,最终得到校正后的状态估计。 根据 k-1 时刻的信息估计 k 时刻的系统状态,称之为“先验估计”——Xk(-),假定系统噪声为零均值,则 k 时刻的状态估计可写为: 在预测了状态的先验估计之后,当获得外部源的测量信息时,先验估计会得到更正。 测量协方差为Rk,表示测量数据的不确定度。 在测量更新环节中,最重要的一步为:计算卡尔曼增益的加权因子 K 当 tk 时刻得到一个新的测量值 Zk 时,将其与基于先验状态估计的预测测量值 HkXk(-) 进行比较。用K对二者误差进行加权,并更新状态向量的预测以生成最佳估计。因此,tk 时刻的状态估计值为: 在大多数INS应用中,KF更新过程的频率低于预测。例如,通过卡尔曼滤波将GPS和INS集成的典型应用中,预测可能以100hz的频率进行,而更新以1hz的频率进行。下图显示了典型组合系统的预测和更的过程: KF由5个步骤组成,算法流程如图所示: 线性系统是理想化的结果,严格地说,几乎所有的系统都是非线性的(甚至欧姆定律也只是在有限范围内的一个近似,它的线性在超过一定的电压阈值之后就会失效(Simon 2006))。 由于许多系统非常接近线性,通过对非线性系统进行线性化,可以将线性估计理论应用于非线性系统。在非线性的卡尔曼滤波中,通常将误差状态作为估计值,而不是以系统状态作为状态估计。在GPS/INS的组合导航中,误差状态是由INS状态和辅助源状态(GPS)之间的差值形成的。可用的方法有线性KF (LKF)和扩展卡尔曼滤波(EKF)。 4.1 线性化kalman滤波对于某一个系统,在标准轨迹附近进行线性化的的方法,被称为线性化卡尔曼滤波(LKF)。此处的标准轨迹通常是已知的,例如,船或客机的航线是预先规划的。对于INS/GPS组合导航系统来说,惯导的输出通常被认为是标准轨迹。 线性化卡尔曼滤波是一个开环结构,其滤波器估计的误差虽然对INS的输出进行校正,但是对INS系统却不构成反馈。 轨迹的真值是无法事先知道的,因此通常将轨迹最优估计的结果作为标准轨迹。 当卡尔曼滤波使用先前最优估计的线性化轨迹,而非预定义的标准轨迹时,这种卡尔曼滤波被称为扩展卡尔曼滤波EKF。 EKF对应于闭环的结构,其滤波器估计的误差将反馈到INS系统以矫正其输出: 对实际系统进行数学表示和线性化的简化过程会导致系统模型的不完善,从而导致KF的发散。这个问题在KF理论的一开始就被发现了,提出的散度控制技术包括在KF过程模型中加入虚拟噪声、Schmidt epsilon技术、有限记忆滤波和渐消记忆滤波。 5.1 KF过程模型中添加虚拟噪声为了使一个真实系统的数学模型易于处理,必须进行许多简化和假设。此外,大多数系统是非线性的,在使用KF之前必须经过线性化,而这种线性化的方法相当于对系统模型又做了进一步的近似。 这一系列不确定因素,可能会造成KF性能下降,甚至发散。 针对这种情况,可以通过增加过程噪声协方差Q 或者 测量噪声协方差R的方式引入人工噪声。 因此,在实际应用中,Q和R的参数设定一般是基于之前经验所得,并通过调整,以优化系统的性能。 5.2 Schmidt Epsilon方法在KF中控制离散度的一种方法是为估计误差的协方差矩阵定义一个阈值,使它不低于某个值。在卡尔曼滤波运行过程中,协方差矩阵Pk(-)不断变小,卡尔曼增益K也不断减小,从公式中可以明显看出,当Pk(-)很小时,K趋近0,新的测量值将无法得到足够的权重,滤波器的输出将主要取决于预测值。这一现象被称为Kalman filter incest 。 在协方差阈值化中,通过给计算得到的误差协方差矩阵Pk(-)加上一个符号为正的小量 ,就可以确保卡尔曼增益存在一个下界。这个小量值是根据经验确定: 在kalman滤波中,最新的估计值Xk(+)是基于从起始时刻到 k 时刻的所有测量值。为了得到最优估计值,滤波器必须考虑到之前的所有测量值。 但是,旧的数据有时不仅无法提供有效信息,反而会导致滤波器出现问题。 因此,在渐消记忆滤波方法中仅使用最近的测量数据,旧的信息将被丢弃。 例如,仅使用测量的最后100个样本进行滤波。 5.4 渐消记忆滤波有时,一些系统模型只在有限时间内是有效的,在超过一定时限之后,系统模型会发生变化。 针对这种情况,可以通过逐渐减小测量噪声的协方差R的方式,来丢弃旧的测量值,以限制其对最新估计值的影响。 这一思想可以通过引入渐消因子来表示: 同样,将渐消因子a引入误差协方差的计算公式中,也可以起到同样的效果: 考虑一个匀加速直线运动的物体。 速度误差随时间的变化率等价于加速度误差,同样,位置误差随时间的变化率等于速度误差,这可以用以下式子表述: 对于无法获得连续更新的测量值的系统 或者在测量临时拒止的条件下,速度误差以及受到速度误差直接影响的姿态分量与位置分量都有可能不断增大。 与单系统相比,使用ZUPT系统在惯性测量精度上有了显著的提高: ZUPT的具体步骤如下: 每2-4分钟停车30-60秒。在停止时获得30—40个速度测量值。将这些测量用于KF更新或速度曲线拟合。 ZUPT的优点是不需要额外的设备,操作简单,成本低廉,预测和测量之间不存在同步问题。 ZUPT的主要限制在于车辆必须立即停止,但这在实际运用中是无法实现的。 6.2.1 通过ZUPT提高测量精度
ZUPT的重要性包括以下几点: 限制了速度误差的增长,甚至可以将其重置为零。有助于估计加速度计的偏置误差。有助于估计三个姿态角的偏差。 总的来说,ZUPT过程在限制测量误差长期增长方面是有效的。 6.3 坐标更新在某些导航应用中,可以提供连续的坐标更新,或者是由坐标已精确预定义的CUPT站点提供。 使用CUPT的过程中,物体运动将被临时中断,使得INS的输出可以与辅助源提供的测量坐标进行比较。 在某些应用程序中,坐标更新可以连续提供,也可以在坐标已精确预定义的CUPT站点提供。如果导航系统作为一个独立的INS而不进行更新,我们预计长期的误差会显著增加,即使在使用高度精确的导航级惯性传感器的情况下也是如此。CUPT。物体的运动被中断,以允许INS的输出与辅助源提供的坐标测量进行比较 6.3.1 通过CUPT提高测量精度CUPT整个过程类似于ZUPT,只是更新的是位置坐标而不是速度。其主要目的在于校正INS的位置误差:在每一CUPT时刻,INS输出的位置与CUPT间的差异将作为卡尔曼滤波器的输入,如下图所示: |
今日新闻 |
推荐新闻 |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |