本发明涉及滤波容错算法，具体涉及一种基于模型预测滤波的动态自适应扩展卡尔曼滤波（ekf）容错算法。
背景技术：
目前，随着科技的迅速发展，复杂系统的故障隐患和不确定性因素不断增加，严重影响着卡尔曼滤波的应用。卡尔曼滤波作为一种信息融合滤波算法，被应用到各种领域。工程上组合导系统采用的滤波方法主要是卡尔曼滤波(kalmanfilter，kf)和扩展卡尔曼滤波(extendedkalmanfilter，ekf)。卡尔曼滤波要求系统数学模型必须为线性，为了解决非线性问题在卡尔曼的基础上提出了扩展卡尔曼滤波。然而，由于系统本身元器件的不稳定性及外部应用环境不确定因素的影响，系统和观测噪声的统计特性难以准确描述，所以卡尔曼滤波器在实际应用中并不能保证其收敛性和稳定性，故障容错性能较差。为了提高卡尔曼滤波的容错性，国内外学者对此做了大量的研究，如遗传模糊推理自适应ukf滤波算法、鲁棒神经网络容错滤波算法、采用双状态传播卡方检验和模糊自适应的容错滤波算法等，以上方法虽然从不同程度上提高了滤波的容错能力，但也存在一些不足，如鲁棒神经网络难以同时精确估计模型和量测噪声，且状态阶数较高时容易发散；强跟踪滤波中衰减因子估计为次优，计算比较繁琐。
模型预测滤波器（modelspredictivefilter,mpf）是crassidis等人基于mme准则和预测控制理论提出的一种实时的非线性滤波技术。模型预测滤波器对系统模型误差没有任何限制，因此可以采用mpf方法来实时估计系统实际的模型误差。基于此，本发明结合预测滤波和扩展卡尔曼滤波提出一种基于模型预测滤波的动态自适应ekf容错算法，一方面，利用模型预测滤波估计不确定系统的模型误差，并对其进行修正，以弥补模型误差对滤波器状态更新的影响，提高系统的输出精度。另一方面，通过模型预测滤波引入滤波因子实现ekf滤波增益的动态调整，以提高其容错能力。
技术实现要素：
本发明针对上述问题，提供了一种基于模型预测滤波的动态自适应扩展卡尔曼滤波（ekf）容错算法。
本发明采用的技术方案是：一种动态自适应扩展卡尔曼滤波容错算法，包括以下步骤：
s1，对系统模型误差进行估计，用于修正误差；
s2，对系统状态向量进行预测；
s3，对系统状态协方差矩阵进行预测；
s4，计算卡尔曼滤波增益矩阵；
s5，构造动态调整自适应因子；
s6，对系统状态进行更新；
s7，系统状态更新协方差。
进一步地，所述步骤s1具体为：
对系统模型误差进行估计，用于修正误差：
模型误差估计的公式简化项；
统下一步的输出向量，
更进一步地，所述步骤s2具体为：
对系统状态向量进行预测：
其中，
更进一步地，所述步骤s3具体为：
对系统状态协方差矩阵进行预测：
其中，
更进一步地，所述步骤s4具体为：
计算卡尔曼滤波增益矩阵：
其中，
更进一步地，所述步骤s5具体为：
构造动态调整自适应因子
其中，
更进一步地，所述步骤s6具体为：
对系统状态进行更新：
其中，
更进一步地，所述步骤s7具体为：
系统状态更新协方差：
其中，
本发明的优点：
本发明利用模型预测滤波估计不确定系统的模型误差，并对其进行修正，以弥补模型误差对滤波器状态更新的影响，提高系统的输出精度；通过模型预测滤波引入滤波因子实现ekf滤波增益的动态调整，以提高其容错能力。
除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
图1是本发明的动态自适应扩展卡尔曼滤波容错算法流程图；
图2是本发明的东向位置误差曲线图；
图3是本发明的北向位置误差曲线图；
图4是本发明的速度误差仿真曲线图；
图5是本发明的车载实验现场图图；
图6是本发明的gps/ins组合定位系统车载测试路线图；
图7是本发明的车载测试经度数据处理前后对比图；
图8是本发明的ekf算法滤波误差分布图；
图9是本发明的mpekf算法滤波误差分布图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
参考图1，如图1所示，一种动态自适应扩展卡尔曼滤波容错算法，包括以下步骤：
s1，对系统模型误差进行估计，用于修正误差；
s2，对系统状态向量进行预测；
s3，对系统状态协方差矩阵进行预测；
s4，计算卡尔曼滤波增益矩阵；
s5，构造动态调整自适应因子；
s6，对系统状态进行更新；
s7，系统状态更新协方差。
本发明利用模型预测滤波估计不确定系统的模型误差，并对其进行修
正，以弥补模型误差对滤波器状态更新的影响，提高系统的输出精度；通过模型预测滤波引入滤波因子实现ekf滤波增益的动态调整，以提高其容错能力。
所述步骤s1具体为：
对系统模型误差进行估计，用于修正误差：
模型误差估计的公式简化项；
统下一步的输出向量，
所述步骤s2具体为：
对系统状态向量进行预测：
其中，
所述步骤s3具体为：
对系统状态协方差矩阵进行预测：
其中，
所述步骤s4具体为：
计算卡尔曼滤波增益矩阵：
其中，
所述步骤s5具体为：
构造动态调整自适应因子
其中，
所述步骤s6具体为：
对系统状态进行更新：
其中，
所述步骤s7具体为：
系统状态更新协方差：
其中，
本发明通过模型预测滤波对系统模型误差进行实时估计，修正系统模型误差，并且利用模型预测滤态调整卡尔曼滤波增益矩阵以提高容错能力、输出精度。
算法仿真与测试：
算法仿真：
为验证本发明提出算法的有效性，通过matlab搭建以gps/ins组合导航系统为仿真平台，并采用基于伪距/伪距率的紧耦合方式，状态量取：
其中，
x为相应的经、纬度高度方向的位置误差以及东、北、天速度误差、姿态角误差、陀螺仪随机常值漂移误差及其-阶马尔可夫过程、加速度计误差、与时钟频率误差等效的距离误差、与时钟误差等效的距离误差等参数构成的状态向量。
假设参与组合的gps卫星数量为4，观测量取
z表示ins和gps的伪距。
其中，
系统仿真参数设置为：卫星伪距、伪距率残差取值范围为0到10；与对应的卡尔曼滤波增益因子调整范围为1到0.04，当伪距、伪距率测量残差取值最大值10时，对应的增益因子取最小值0.04。
对matlab搭建的gps/ins组合系统仿真平台进行1000s仿真，部分初始条件如下：运动载体初始位置：纬度36.03，经度103.40；各个方向的运动初始速度为1000（m/s）；东和北的初始误差角均为0.01；经度、纬度的始位置误差为0.2；初始速度误差0.2m/s；陀螺常值和随机漂移：0.09/h,0.04/h；相关时间2h；加速度计均值误差110ug；载体加速度：
为了检验本算的容错效果，从501s时刻引入故障噪声，即将测量噪声扩大为初始值的10倍，为说明本算法的优越性，现将本算法和标准的ekf滤波算法比较，其结果如图所示。其中图2为东向位置误差曲线；图3为北向位置误差曲线；图4为速度误差曲线。从图中可以看出与传统ekf算法相比，本发明提出的基于模型预测滤波的动态自适应ekf容错算法（mpekf）具有更好的容错能力与滤波效果。
车载测试分析：
为进一步验证本发明提出算法的有效性和可用性，采用gps卫星接收板卡（k700）和imu惯性测量单元（mpu6050）搭建gps/ins组合定位平台如图5所示，测试路线如图6所示。
从图中可以看出本算法处理后的定位精度得到了提高，为进一步说明数据的科学性，将测量误差进行进一步处理其结果如图8和9所示。
其中，图8是传统ekf算法的处理后的误差分布，根据图7数据计算其统计方差为：
d表示样本方差，e表示样本期望。
其中，图9是mpekf算法的处理后的误差分布，根据图9数据计算其统计方差为：
因此，可以得出改进后的ekf其滤波精度提高了：
本发明通过仿真及实物测试分析，可以得出引入模型预测滤波的ekf算法其容错能力及滤波精度都得到了很大的提升，为以后ekf算法的进一步应用提供一定的参考价值。
本发明通过gps/ins组合导航平台进行测试仿真。结果表明，改进后的扩展卡尔曼滤波具有更强的容错性能及总体精度，滤波性能明显优于扩展卡尔曼滤波算法。
本发明针对系统误差的不确定性导致滤波容错性能不足的问题，提出一种基于模型预测滤波的动态自适应扩展卡尔曼滤波（ekf）容错算法。一方面，利用模型预测滤波估计不确定系统的模型误差，并对其进行修正，以弥补模型误差对滤波器状态更新的影响，提高系统的输出精度。另一方面，通过模型预测滤波引入滤波因子实现ekf滤波增益的动态调整，以提高其容错能力。通过gps/ins组合导航平台进行测试仿真。
结果表明，改进后的扩展卡尔曼滤波具有更强的容错性能及总体精度，滤波性能明显优于扩展卡尔曼滤波算法。
本发明的一种基于模型预测滤波的动态自适应ekf容错算法，通过误差模型预测以及引入卡尔曼增益因子来解决滤波器容错性能不足、精度差的问题。通过matlab模拟仿真以及实物测试进行了验证，结果表明模型预测滤波动态自适应ekf算法能在提高滤波精度的前提下，具有比传统扩展卡尔曼滤波更强的容错能力。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。