1、3 卡尔曼滤波方法3.1 卡尔曼滤波的特点及应用领域3.2 系统的状态空间描述3.3 卡尔曼滤波的直观推导3.4 卡尔曼滤波的递推运算方程3.5 卡尔曼滤波的结构图3.6 卡尔曼滤波的应用实例3.7 联邦卡尔曼滤波3.8 联邦卡尔曼滤波的应用实例3.9 Unscented卡尔曼滤波23.1 卡尔曼滤波的特点及应用领域卡尔曼滤波卡尔曼滤波(Kalman Filtering)是是1960年由年由R.E.Kalman首首次提出的一种估计方法。之所以称为滤波，是因为它是一次提出的一种估计方法。之所以称为滤波，是因为它是一种排除随机干扰，提高检测精度的一种手段。种排除随机干扰，提高检测精度的一种手段。K
2、F是基于是基于最小方差准则最小方差准则推导出来的一种线性滤波器。推导出来的一种线性滤波器。KF是一种时域是一种时域递推算法递推算法，根据上一状态的估计值和当前，根据上一状态的估计值和当前状态的观测值推出当前状态，不需存储大量的历史数据，状态的观测值推出当前状态，不需存储大量的历史数据，便于计算机实现。便于计算机实现。KF要求明确已知系统模型。即在应用卡尔曼滤波之前，要求明确已知系统模型。即在应用卡尔曼滤波之前，首先要建立首先要建立系统模型系统模型和和观测模型观测模型，并假定过程噪声、观测，并假定过程噪声、观测噪声为高斯白噪声。噪声为高斯白噪声。应用领域：机器人导航、目标跟踪、组合导航等。其中，
3、应用领域：机器人导航、目标跟踪、组合导航等。其中，组合导航组合导航是卡尔曼滤波最成功的应用领域。是卡尔曼滤波最成功的应用领域。33.2 系统的状态空间描述连续系统模型：连续系统模型：-状态方程-观测方程维维向向量量函函数数。维维和和维维随随机机测测量量噪噪声声；维维系系统统随随机机干干扰扰；维维观观测测向向量量；维维状状态态向向量量；mnthtfmtVptWmtZntX)(),()()()()(4系统的状态空间描述(续)53.3 卡尔曼滤波的直观推导则设想：则设想：，的最优线性估计的最优线性估计且已得到且已得到次测量结果次测量结果时刻已获得时刻已获得假设在假设在1|11121,kkkkkXXZ
4、ZZZkk1|1|1|11,1|kkkkkkkkkkkXHZXX 得得预测测量估计偏差预测测量估计偏差：1|1|1|kkkkkkkkkXHZZZZ 利用此偏差利用此偏差修正预测估计修正预测估计：11|kkkkkkkkkXHZKXX待定校正增益阵6卡尔曼滤波的直观推导(续)增益阵的求法增益阵的求法：极极小小化化使使目目标标函函数数|kkTkkXXEkkkkkkkkkkXXXXXX|11|定义：定义：滤滤波波器器的的递递推推运运算算方方程程卡卡尔尔曼曼求求得得增增益益阵阵，进进而而求求得得极极小小，使使|TkkkkkkXXEP73.4 卡尔曼滤波的递推运算方程111,1kkkkkkXX状态预测Tk
5、kkkkTkkkkkkkkQPP1,11,1,111,1方差预测111kTkkkkTkkkkRHPHHPK增益矩阵11|kkkkkkkZKXX状态估值方差估值1kkkkkkPHKIP时间更新/预测测量更新/修正000000PPXX初始条件11kkkkkkXHZZ新息序列83.5 卡尔曼滤波的结构图延时一步1,kkkZ+-1|kkZkKkX+当前估计值上一步估计值1|kkXkH一步预测1kX 上述递推公式，称为卡尔曼滤波器。实际上，卡尔曼上述递推公式，称为卡尔曼滤波器。实际上，卡尔曼滤波器也是一个系统，其结构框图如下：滤波器也是一个系统，其结构框图如下：93.6 卡尔曼滤波的应用实例(舰船导航)
6、状态变量状态变量TENKSVVkX)(1 1、系统模型、系统模型 卡尔曼滤波最成功的工程应用是设计运载体的高精度组合导航卡尔曼滤波最成功的工程应用是设计运载体的高精度组合导航系统。下面以舰船导航问题为例，介绍其具体应用。系统。下面以舰船导航问题为例，介绍其具体应用。式中，式中，舰舰船船航航向向变变化化率率。舰舰船船航航向向；船船舶舶相相对对于于水水的的速速度度；量量；海海流流北北向向和和东东向向速速度度分分和和经经度度弧弧长长和和纬纬度度弧弧长长；和和KSVVEN10 为独立的零均值高为独立的零均值高斯白噪声斯白噪声,b为海流相关时间常数。为海流相关时间常数。)7,2,1(ii状态方程状态方程
7、7654321sincos,KSbVVbVVKSVKSVWtXfXNNEEEN43214321vKvSvvzzzz观测方程观测方程 量测系统由量测系统由GPS和航位推算系统和航位推算系统(DR)组成，组成，GPS输出舰船的经纬度输出舰船的经纬度和和。DR由罗经和计程仪组成，分别输出由罗经和计程仪组成，分别输出航向航向K和船相对于水流的速度和船相对于水流的速度S。112 2、初始条件和参数选取、初始条件和参数选取123 3、仿真结果、仿真结果(a)(a)133 3、仿真结果、仿真结果(b)(b)143 3、仿真结果、仿真结果(c)(c)153.7 联邦卡尔曼滤波卡尔曼滤波最成功的工程应用是设计运
8、载体的高精度组合卡尔曼滤波最成功的工程应用是设计运载体的高精度组合导航系统。为了与联邦滤波方法相区别，将普通的卡尔曼导航系统。为了与联邦滤波方法相区别，将普通的卡尔曼滤波称为集中卡尔曼滤波。滤波称为集中卡尔曼滤波。由于对导航精度要求的提高，导航设备越来越多。另一方由于对导航精度要求的提高，导航设备越来越多。另一方面，现代系统向大系统和复杂系统的方向发展。这种情况面，现代系统向大系统和复杂系统的方向发展。这种情况下采用集中式卡尔曼实现组合导航，存在两个问题：下采用集中式卡尔曼实现组合导航，存在两个问题：计算负担重。滤波器计算量以状态维数的三次方剧增，无法满足导计算负担重。滤波器计算量以状态维数的
9、三次方剧增，无法满足导航的实时性要求；航的实时性要求；容错性能差，不利于故障诊断。当任一导航子系统发生故障，且没容错性能差，不利于故障诊断。当任一导航子系统发生故障，且没有及时检测出并隔离时，则整个导航系统都会被污染，使输出的信有及时检测出并隔离时，则整个导航系统都会被污染，使输出的信息不可靠。息不可靠。为解决上述问题，出现了为解决上述问题，出现了分散化滤波分散化滤波的思想和方法。其中，的思想和方法。其中，Carlson在在1988年提出的联邦滤波由于设计灵活、计算量小、年提出的联邦滤波由于设计灵活、计算量小、容错性好而受到重视，已被美国空军确定为新一代导航系容错性好而受到重视，已被美国空军确
10、定为新一代导航系统的通用滤波器。统的通用滤波器。16 联邦滤波器示意图子滤波器子滤波器1状态估计状态估计1主滤波器主滤波器信息融合信息分配全局状态估计全局状态估计子滤波器子滤波器2子滤波器子滤波器n状态估计状态估计2状态估计状态估计n17 方法思想在诸多非相似导航子系统中选择导航信息全面、输出速率在诸多非相似导航子系统中选择导航信息全面、输出速率高、可靠性好的子系统（如惯性导航系统）与其余导航子高、可靠性好的子系统（如惯性导航系统）与其余导航子系统两两结合，形成若干子滤波器。系统两两结合，形成若干子滤波器。各子滤波器并行运行，获得建立在局部量测基础上的局部各子滤波器并行运行，获得建立在局部量测
11、基础上的局部最优估计。最优估计。各局部最优估计在第二级滤波器（即主滤波器）内按融合各局部最优估计在第二级滤波器（即主滤波器）内按融合算法合成，获得建立在所有量测基础上的全局估计。算法合成，获得建立在所有量测基础上的全局估计。全局估计再按信息守恒原则反馈给各子滤波器。如此反复全局估计再按信息守恒原则反馈给各子滤波器。如此反复递推。递推。F 实际设计的联邦滤波器是全局次优的，但是对于自主性要求很高的重要运载体来说，导航系统的可靠性比精度更重要。采用联邦滤波器设计组合导航系统，虽然损失了少许精度，但得到的是组合导航系统的强容错能力。18 联邦滤波器算法信息融合信息融合其中，其中，为为 N 个不相关的
12、局部状态估计，个不相关的局部状态估计，为相应的估计误差协方差为相应的估计误差协方差。NXXX,21NNPPP,2211,11NiiiiggXPPX111NiiigPP 对于来自各子滤波器信息（状态估计值和误差方差对于来自各子滤波器信息（状态估计值和误差方差估计），在主滤波器按以下规则进行融合：估计），在主滤波器按以下规则进行融合：19信息分配信息分配,giXX,1giiiPP1111mNiiQQQ其中，其中，为信息分配系数，为信息分配系数，为主滤波器的信息分配系数，满足守恒原则为主滤波器的信息分配系数，满足守恒原则mNii,1,11mNii,1QQmim 在进入下一次递推之前，需将主滤波器中的
13、信息在进入下一次递推之前，需将主滤波器中的信息（状态、方差）在各子滤波器中按如下规则进行分配：（状态、方差）在各子滤波器中按如下规则进行分配：联邦滤波器算法20 联邦卡尔曼滤波器结构信息分配卡尔曼滤波信息融合联邦卡尔曼滤波器21 联邦卡尔曼滤波器结构图 注：组合导航中，一般选择可靠性好的系统作为参考注：组合导航中，一般选择可靠性好的系统作为参考系统，如惯性导航系统。系统，如惯性导航系统。22联邦卡尔曼滤波器的应用01002003004005006007008009001000-150-100-50050100150联 邦 卡 尔 曼 滤 波 算 法集 中 卡 尔 曼 滤 波 算 法时间/s经度
14、弧长估计误差经度弧长估计误差/m 仍然以舰船导航为例，模型与前面相同。为了与集中卡仍然以舰船导航为例，模型与前面相同。为了与集中卡尔曼滤波器的效果相比较，同时采用两种滤波器滤波。尔曼滤波器的效果相比较，同时采用两种滤波器滤波。01002003004005006007008009001000-200-150-100-50050100150联 邦 卡 尔 曼 滤 波 算 法集 中 卡 尔 曼 滤 波 算 法维度弧长估计误差维度弧长估计误差/m时间/s23联邦卡尔曼滤波器的应用(续)图中可以看出，联邦卡尔曼滤波与集中卡尔曼滤波算法图中可以看出，联邦卡尔曼滤波与集中卡尔曼滤波算法的估计精度相当，但联邦
15、卡尔曼滤波相对于集中卡尔曼滤的估计精度相当，但联邦卡尔曼滤波相对于集中卡尔曼滤波，不仅有更好的容错性，而且可以并行运算。波，不仅有更好的容错性，而且可以并行运算。1002003004005006007008009001000-0.8-0.6-0.4-0.200.20.40.6联 邦 卡 尔 曼 滤 波 算 法集 中 卡 尔 曼 滤 波 算 法海流北向速度分量估计误差海流北向速度分量估计误差/(m/s)时间/s1002003004005006007008009001000-0.5-0.4-0.3-0.2-0.100.10.20.30.4联 邦 卡 尔 曼 滤 波 算 法集 中 卡 尔 曼 滤 波
16、 算 法海流东向速度分量估计误差海流东向速度分量估计误差/(m/s)时间/s243.9 Unscented卡尔曼滤波 UKF方法的建立是基于如下的事实：逼近任意的分布比方法的建立是基于如下的事实：逼近任意的分布比逼近它的任何非线性函数更容易。逼近它的任何非线性函数更容易。UKF的基础是的基础是Unscented变换变换(Unscented Transformation,UT)，其基本思想是用一组，其基本思想是用一组确定的离散采样点确定的离散采样点(称为称为Sigma点点)来近似状态变量的分布。来近似状态变量的分布。UKF假定状态满足高斯分布，因此只需逼近其均值和方差。假定状态满足高斯分布，因此
17、只需逼近其均值和方差。对于如下的非线性变换：对于如下的非线性变换：假设已知假设已知n维随机向量维随机向量 x 的均值的均值 和协方差和协方差 ，则对上述，则对上述非线性系统进行滤波的目的是计算非线性系统进行滤波的目的是计算 y 的均值的均值 和和 方差方差 。xxPyyP)(xfy-(3-1)250,2,1,)(,1,)(ixnniPnxniPnxixixi1.构造构造 Sigma 点点ixxPxx构构造造一一组组采采样样点点的的附附近近在在，和和协协方方差差的的均均值值根根据据随随机机向向量量0为可调尺度参数，调整它可以提高逼近精度。为可调尺度参数，调整它可以提高逼近精度。2.利用利用Sig
18、ma点进行非线性传播点进行非线性传播 将上面构造的将上面构造的Sigma点直接按照点直接按照(3-1)的关系作非线性的关系作非线性变换，则产生一组变换样本点变换，则产生一组变换样本点)(iifY26Unscented卡尔曼滤波(续)变换样本点变换样本点Yi 即可近似表示即可近似表示 y 的分布。下面利用的分布。下面利用 Yi 来计算来计算 y的均值和方差。的均值和方差。3.计算计算 y 的均值和方差的均值和方差ipimiYWy0)(TiipiciyyYyYWP)(0)(nininWWcimi2,1),(2/10),/()()(其中，其中，27UKF的具体应用过程 Unscented卡尔曼滤波算
19、法基于如下的非线性离散状态卡尔曼滤波算法基于如下的非线性离散状态空间模型：空间模型：kkkkwxfx)(1kkkkvxhy)(时间更新时间更新与卡尔曼滤波一样，与卡尔曼滤波一样，Unscented卡尔曼滤波也是由卡尔曼滤波也是由“时间更时间更新新”和和“测量更新测量更新”构成，具体如下：构成，具体如下：)(11|ikkikkfniikkmikWx201|)(kkikknikikkcikxQxxWPT1|201|)(,28UKF的具体应用过程(续)测量更新测量更新，)(1|1|ikkikkhikknimikWy1|20)(kTkikkkniikkcikyRyyWP1|201|)(,TkikkkikknicikxyyxWP1|1|20)(,1,kykxyPPK)(kkkkyyKxxTkykxkxKKPPP,
