第五讲：卡尔曼滤波-65页PPT文档 (2)PPT文件格式下载.pptx

第五讲：卡尔曼滤波-65页PPT文档 (2)PPT文件格式下载.pptx

《第五讲：卡尔曼滤波-65页PPT文档 (2)PPT文件格式下载.pptx》由会员分享，可在线阅读，更多相关《第五讲：卡尔曼滤波-65页PPT文档 (2)PPT文件格式下载.pptx（65页珍藏版）》请在冰点文库上搜索。

卡尔曼滤波算法使用观测向量、观测模型和系统模型来获得状态向量的最优估计，分为系统传递和测量更新两个部分。

目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,目录,11,1.5卡尔曼滤波的导航应用,惯性导航系统（INS）的精对准和标定单一导航（GNSS,无线电、水声学、匹配）组合导航INS/GNSS组合导航及多传感器组合导航INS/水声组合导航INS/匹配导航,概述经典KFEKFLKF,二、Kalman滤波,2023/4/28,12,13,2.1卡尔曼滤波方程,1.离散系统的数学描述设离散化后的系统状态方程和量测方程分别为：

Xk为k时刻的n维状态向量（被估计量）,Zk为k时刻的m维量测向量,k-1到k时刻的系统一步状态转移矩阵（nn阶）,Wk-1为k-1时刻的系统噪声（r维）,k-1为系统噪声矩阵（nr阶）,Hk为k时刻系统量测矩阵（mn阶）,Vk为k时刻m维量测噪声,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,14,要求Wk和Vk是互不相关的、零均值白噪声序列：

Qk和Rk分别称为系统噪声和量测噪声的方差矩阵，分别是已知值的非负定阵和正定阵；

kj是Kronecker函数，即：

目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,15,初始状态的一、二阶统计特性为：

Var为对求方差的符号,卡尔曼滤波要求mx0和Cx0为已知量，,且要求X0与Wk和Vk都不相关,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,16,2.离散卡尔曼滤波方程,或,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,17,时间更新方程,量测修正方程,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,18,滤波计算回路,增益计算回路,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,19,3.卡尔曼滤波示例,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,有一个质点，沿X轴正方向运动，质点从X=0开始匀速直线运动,速度为V=10m/s，则每一时刻质点的真实位置（参考真值）为：

X=X0+V*t；

实际上，我们每隔0.1s可以测量一次质点的位置，但位置测量值存在误差（假设是均值为0的白噪声序列）根据我们对质点的位置观测量，用卡尔曼滤波方法计算每一时刻质点的位置和速度,20,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,状态量x=X,V，即以质点的位置和速度作为卡尔曼滤波状态量；

系统状态方程为Xk=Xk-1+Vk-1*dt；

状态转移矩阵Phi=1dt;

01；

系统噪声协方差阵Q：

即系统模型的不确定度，由于假设模型即质点运动模型，因此可认为模型的不确定度为0，即Q=00;

00观测矩阵H：

由于只观测了质点位置，未观测速度，因此观测矩阵H=10;

观测噪声矩阵R：

位置观测量的方差为m2，即R=1观测量向量Z：

在真实状态（真实位置）加上均值为零，方差为m2的白噪声;

卡尔曼滤波初始状态：

X0=0,V0=5m/s，初始状态误差协方差矩阵P=10;

01,设计卡尔曼滤波,21,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,卡尔曼滤波位置估计,22,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,卡尔曼滤波速度估计,23,2.2闭环卡尔曼滤波,1.全状态滤波和误差状态滤波根据卡尔曼滤波状态向量的选取不同，卡尔曼滤波可分为:

全状态卡尔曼滤波（TotalStateImplementation）和误差状态卡尔曼滤波（ErrorStateImplementation）,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,24,TotalState：

以系统的固有属性，如位置、速度和姿态等，为状态向量的卡尔曼滤波，称为全状态滤波或直接卡尔曼滤波ErrorState：

以系统测量误差值，如INS位置、速度和姿态等，为状态向量的卡尔曼滤波称为误差状态滤波或间接卡尔曼滤波,直接法,间接法,以各种导航参数X为主要状态,滤波器估值的主要部分即是导航参数的估值,以某种导航系统输出导航参数的误差为主要状态,滤波器估值的主要部分即是导航参数误差的估值,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,模型可能是线性的，也可能是非线性的,模型一般都是线性的,25,2.开环卡尔曼滤波,25,用导航参数误差的估值去校正系统输出的导航参数，得到综合导航系统的导航参数估值,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,开环（输出校正）的卡尔曼滤波器,26,3.闭环卡尔曼滤波,26,采用反馈校正的间接法估计，是将惯导系统导航参数误差的估值反馈到惯导系统内，对误差状态进行校正。

目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,闭环（反馈校正）的卡尔曼滤波器,27,27,开环滤波仅校正系统输出量，闭环滤波则是校正系统内部的状态。

两种校正方法的性质是一样的，具有同样的精度。

闭环滤波的反馈校正使得卡尔曼滤波状态值为小量；

开环因无反馈，状态值会随时间不断变大。

状态方程都是经过一阶近似的线性方程，状态的数值越小，则近似的准确性越高，因此，利用状态反馈校正的系统状态方程，更能接近真实地反映系统误差状态的动态过程。

卡尔曼滤波算法中，反馈状态估计的最佳时机是在测量更新后立即进行。

卡尔曼滤波的闭环和开环可以混合使用，即一些状态估计作为校正值被反馈，而另外一些不反馈。

目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,4.开环与闭环卡尔曼滤波对比,28,5.混合卡尔曼滤波示例,28,在松组合算法中，21维向量，其中：

位置、速度和姿态只做开环修正；

而IMU误差，如陀螺和加速度计零偏，比例因子误差进行反馈，修正IMU的原始观测值。

目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,GINS软件松组合算法架构示意图（混合滤波）,29,2.3滤波特性及滤波实现中的问题,1.滤波收敛特性,收敛过程中的卡尔曼滤波状态不确定度,*注：

状态不确定度是误差协方差矩阵P对角元素的平方根,当卡尔曼滤波状态不确定度接近平衡点，每次测量更新后状态不确定度的降低量与系统噪声造成的不确定度的增加量是匹配的；

在平衡点，不确定度所反映出的估计置信度水平基本固定.,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,30,状态估计的收敛速度基本上取决于该状态的可观测性。

如果观测矩阵随时间变化或者状态之间通过状态转移矩阵存在时间依存关系，那么随着迭代次数的增加，更多的状态量变得可观测。

例：

导航中用位置的变化率来确定速度许多参数的可观测性依赖于系统的动态性例：

姿态不变时，INS姿态误差和加速度计偏差不是独立可观测的；

陀螺仪误差则需要载体有更高的动态性，方可观测如果两个状态对观测量有同样的影响，以相同方式随时间变化，并且具有相同的动态特性，则它们非独立可观量，在滤波设计时，应将其组合在一起，以免浪费计算资源,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,31,2.滤波参数调整,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,不同P/R比率下的卡尔曼滤波误差传递（PaulGroves）,为克服卡尔曼滤波模型的限制，噪声建模必须足以囊括实际系统的行为，形象地说，要将真实世界的“方形楔子”放到卡尔曼滤波模型的“圆洞”中，因此这个洞要被扩宽。

若忽略1HzGPS定位结果误差的时间相关性，应将其放大（一般为2-3倍）以建模为白噪声,32,P阵非正定P阵的非正定容易导致滤波发散。

使用高精度变量存储（如double类型），减小舍入误差；

缩放卡尔曼滤波标度，使所有状态不确定度在数值上具有相同量级；

P阵非对称使用式计算P阵，容易导致P阵非对称；

每次系统传递及测量更新后通过P=（P+P）/2来保持对称性；

建议采用Joseph式P阵更新：

平方根滤波传递而非P,可把动态范围减小两个量级，从而减小舍入误差影响。

目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,3.数值计算问题,三、扩展Kalman滤波（EKF）,2023/4/28,33,34,标准卡尔曼滤波的线性假设在标准的卡尔曼滤波中，观测模型假设为线性（Z是X的线性函数），但实际情况往往并非如此（如GNSS导航滤波器中，观测模型是强非线性的）在标准卡尔曼滤波中，系统模型也被假设为线性的（X的时间导数是X的线性函数）问题：

在全状态INS/GNSS组合导航中，状态量为绝对位置、速度和姿态，因为难以一直保持系统的线性近似，完成所有的系统反馈并不总是可行的；

扩展/线性化卡尔曼滤波为卡尔曼滤波的非线性形式,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,1.问题描述,3.1非线性系统,35,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,2.非线性系统描述,一般非线性系统（连续）和离散系统可由以下方程来表示：

扩展卡尔曼滤波研究的非线性系统可由以下方程来表示,或,为简化问题，需对噪声的统计特性给以符合实际又便于处理的假定,或,Wt或Wk-1，Vt或Vk均为彼此不相关的零均值白噪声序列，与初始状态X（0）或X0也不相关,36,36,如何解决非线性问题,采用近似的方法,线性化方法,目前应用比较广泛的是,对非线性模型的线性化,非线性系统能否应用卡尔曼滤波的关键,基本假设：

非线性微分方程的理论解一定存在；

理论解与实际解差能够用一个线性微分方程表示，即线性扰动方程。

目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,3.解决方案,37,3.2非线性系统的线性化,t,实际状态X,标称状态X*,围绕X*线性化：

线性化卡尔曼滤波（LinearizedKalmanFilter，LKF）围绕实际状态X（滤波估计状态，或实际轨迹）进行线性化：

扩展卡尔曼滤波（ExtendedKalmanFilter，EKF）,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,泰勒展开法进行线性化,状态,38,线性化卡尔曼滤波：

围绕标称状态对非线性系统进行线性化,或,当Wt或Wk-1，Vt或Vk恒为0时，系统模型的解称为非线性方程的理论解，又称“标称轨迹”或标称状态。

通常记为Xn（t）或Xkn,和Zn（t）或Zkn，则有：

目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,39,3.3线性化卡尔曼滤波,或,定义：

非线性系统的真轨迹运动与标称轨迹运动的偏差为：

如果这些偏差足够小，那么，可以围绕标称状态把X（t）和Z（t）展开成泰勒（Taylor）级数，并且可取一次近似值。

目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,40,3.3线性化卡尔曼滤波,40,或,则有：

非线性系统能否应用卡尔曼滤波的关键,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,由于线性化卡尔曼滤波存在上述问题，改用另一种近似方法，即在最优化状态估计或附近进行泰勒展开，线性化。

41,3.4扩展卡尔曼滤波,41,标称解难以解算真轨迹与标称轨迹之间的状态差X（t）或Xk不能确保其足够小，从而导致线性化误差较大，模型的线性近似度变弱。

LKF的缺陷,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,扩展卡尔曼滤波,目录,42,概述KF方程EKFLKF,42,或,定义非线性系统的真轨迹运动与实际轨迹运动的偏差为：

若偏差足够小，则可以围绕最优化状态估计把X（t）和Z（t）展开成泰勒（Taylor）级数，并且可取一次近似值。

43,或,则有：

EKF假设状态向量估计的误差远比状态向量本身小，因此可用线性系统模型计算状态向量残差。

标准的误差协方差传递公式可采用在最优状态估计附近进行线性化的状态转移矩阵，即,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,四、Schmidt卡尔曼滤波,2023/4/28,44,45,4.1时间相关噪声,45,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,1.观测噪声,标准卡尔曼滤波的观测噪声假设一般假设所有的观测噪声是时间不相关的，即观测噪声是白噪声。

问题：

该假设常不成立；

卡尔曼滤波强制把新息中的时间相关部分归因状态，因此，时间相关的观测噪声可能会破坏状态估计。

处理观测噪声时间相关性问题的方法将时间相关噪声扩展至卡尔曼滤波的状态向量中，进行估计；

放大观测噪声协方差矩阵R，进而减小卡尔曼滤波的增益；

利用Schmidt卡尔曼滤波,46,46,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,2.系统噪声,标准卡尔曼滤波的系统噪声假设标准的卡尔曼滤波中，一般假设所有的系统噪声是时间不相关的，即观测噪声是白噪声。

系统常含有显著的系统性噪声和其它时间相关噪声；

这些噪声可能由于可观测性较差未被选为状态量；

但会影响被估计的状态处理系统噪声时间相关性问题的方法当相关时间较短时：

建模为白噪声，但需覆盖会影响卡尔曼滤波收敛的相关噪声；

当相关时间超过1min：

采用带不确定参数的Schmidt卡尔曼滤波；

47,4.2Schmidt卡尔曼滤波,47,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,48,48,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,Schmidt卡尔曼率滤波对应的误差协方差系统传递式为:

将误差协方差P,相关噪声协方差W,相关矩阵U分解为不同传递方程:

49,49,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,类似于观测矩阵H,定义J为待估计参数到观测向量的耦合矩阵,则卡尔曼滤波增益变为:

Schmidt卡尔曼滤波误差协方差,相关噪声和相关矩阵更新为:

50,50,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,五、自适应卡尔曼滤波,2023/4/28,51,52,5.自适应卡尔曼滤波,52,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,1.问题描述,多数应用中，Kalman滤波中的系统噪声协方差阵Q和观测噪声协方差阵R可事先通过系统测量、仿真和实验等方法确定。

在INS/GNSS组合导航中，Q阵常由IMU的性能参数如VRW,ARW，零偏的一阶高斯马尔可夫过程参数确定GNSS噪声由位置的点位中误差或误差建模问题：

有些情况上述参数是无法获取的，例如：

1）MEMSIMU在出厂时未经过严格标定；

2）如果没有飞机武器挂仓的先验振动环境信息，则无法获得观测噪声协方差阵；

解决办法Kalman滤波自行估计矩阵Q或/和R，即自适应卡尔曼滤波,基于新息的自适应估计（InnovationBasedAdaptiveEstimation,IAE），要从测量新息统计中计算Q、R。

计算最后n个测量信息的协方差上述协方差矩阵C用于计算R和/或Q当处理第一组观测新息统计值时，必须提供R和Q初始值，初始值的选定须谨慎,53,53,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,2.基于新息的自适应估计,多模型自适应估计（MultipleModelAdaptiveEstimation,MMAE）利用一组并行的卡尔曼滤波器进行计算，每一个滤波器对应于不同的系统噪声和/或观测噪声协方差矩阵Q和R。

第i个卡尔曼滤波器被分配的概率为随时间推移，最优滤波假设的概率会接近1，而其它的接近0；

为充分利用计算处理能力，可剔除弱的滤波假设，并周期性地细分最强的假设用于精化滤波器参数。

54,54,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,3.多模型自适应估计,完整的状态向量估计和误差协方差计算公式如下：

IAE（新息）和MMAE（多模型）方法比较MMAE计算量大IAE中，Q,R,P和滤波增益均可能是状态估计的函数；

而他们在MMAE滤波器组（标准卡尔曼滤波而非EKF）中是相互独立的，因此MMAE滤波器更趋于稳定,55,55,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,六、平滑算法,2023/4/28,56,57,6.平滑算法,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,1.问题描述,实时处理/后处理差异卡尔曼滤波器是为实时应用而设计的，用于估计给定时刻的系统特性，所用新息是这一时刻之前的系统能够观测量。

当需要在某时间发生后获得系统特性时，卡尔曼滤波仍旧只用1/2的观测新息，因为卡尔曼滤波器没有使用所关注时间点之后的观测量卡尔曼平滑器是卡尔曼滤波器的扩展，不仅适用所关注时间点之前的观测新息，且适用之后的新息；

非实时应用中，平滑处理能够得到更高的精度。

常用平滑算法包含两类主要方法：

1）正向-方向滤波；

2）RTS平滑算法,58,58,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,2.正向-反向滤波算法,通过正向滤波和反向滤波（两个相互独立的滤波器）结果的加权平均，平滑结果由下式给定：

下标f和b分别表示正向和反向；

k表示两个滤波器中的相同时间点,正向-反向卡尔曼平滑算法的状态不确定度,59,59,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,3.RTS平滑算法：

RTS（Rauch,TungandStriebel,RTS）,传统的卡尔曼滤波随时间向前运行，在每次系统传递和测量更新后记录状态向量X、误差协方差阵P及状态转移矩阵，一旦运算到数据的结尾，则开始反向从末尾到起始点进行数据平滑每次迭代的平滑增益为：

平滑后的状态向量和误差协方差阵为：

当需对每个点进行平滑时，RTS算法更有效，若只需对单点进行平滑时，正向-反向算法更有效,陈起金|卡尔曼滤波,60,附录：

卡尔曼滤波示例MATLAB源代码,60,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,参考:

AlexBlekhman,mathworks/matlabcentral/fileexchange%SettruetrajectoryNsamples=100;

dt=.1;

t=0:

dt:

dt*Nsamples;

Vtrue=10;

%XtrueisavectoroftruepositionsofthetrainXinitial=0;

Xtrue=Xinitial+Vtrue*t;

%Motionequations%Previousstate（initialguess）:

Ourguessisthatthetrainstartsat0withvelocitythatequalsto50%oftherealvelocityXk_prev=0;

0.5*Vtrue;

%CurrentstateestimateXk=;

陈起金|卡尔曼滤波,61,附录：

卡尔曼滤波示例MATLAB源代码,61,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,Phi=1dt;

01;

sigma_model=1;

P=sigma_model20;

0sigma_model2;

Q=00;

00;

%Misthemeasurementmatrix.M=10;

sigma_meas=1;

%1m/secR=sigma_meas2;

%=Kalmaniteration%BuffersforlaterdisplayXk_buffer=zeros（2,Nsamples+1）;

Xk_buffer（:

1）=Xk_prev;

Z_buffer=zeros（1,Nsamples+1）;

陈起金|卡尔曼滤波,62,附录：

卡尔曼滤波示例MATLAB源代码,62,目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法,fork=1:

NsamplesZ=Xtrue（k+1）+sigma_meas*randn;

Z_buffer（k+1）=Z;

%KalmaniterationP1=Phi*P*Phi+Q;

S=M*P1*M+R;

goestothemeasurement.%IfKislow,moreweightgoestothemodelprediction.K=P1*M*inv（S）;

P=P1-K*M*P1;

Xk=Phi*Xk_prev+K*（Z-M*Phi*Xk_prev）;

k+1）=Xk;

%ForthenextiterationXk_prev=Xk;

end;

陈起金|卡尔曼滤波,63,附录：

卡尔曼滤波示例MATLAB源代码,63,目录概述标准KF扩展KFSchmidt