自适应滤波和联邦卡尔曼滤波

导航理论与技术教研室2020/5/101测绘学院五系自适应滤波和联邦卡尔曼滤波组合导航导航理论与技术教研室2020/5/102本次课主要内容主要内容：卡尔曼滤波自适应卡尔曼滤波联邦卡尔曼滤波重点和难点：自适应卡尔曼滤波导航理论与技术教研室2020/5/103卡尔曼滤波是一种递推线性最小方差估计。一、卡尔曼滤波kkkkkkkkkkVXHZWXX1111,设离散化后的系统状态方程和量测方程分别为：导航理论与技术教研室4TxyzenuxyzxyzXvvvLh选取15阶状态变量平台误差角速度误差位置误差陀螺随机常值漂移加速度计随机常值偏置2020/5/104一、卡尔曼滤波导航理论与技术教研室5系统状态方程XFXGW状态转移矩阵NS69661515FFF00噪声转移矩阵nb33n33b9393156C0G0C002020/5/105W系统噪声向量Tzyxzyx)t(W一、卡尔曼滤波导航理论与技术教研室6系统量测方程Z(t)VxVyVzxyzTZHXV观测量由GNSS和INS获得的位置速度误差2020/5/106一、卡尔曼滤波导航理论与技术教研室2020/5/107()()()()()()coscoseIeGeenInGnnuIuGuuIGMMnIGNNeIGuvvvMvvvMvvvMZtHtXtVtLLRRLNRLRLNhhhN系统量测方程一、卡尔曼滤波导航理论与技术教研室2020/5/108运载器惯导系统其他导航设备运载器的运动导航参数导航参数导航参数最优估计卡尔曼滤波器+-输出校正反馈校正+-间接法滤波示意图XˆIXˆNIXXNXIX输出校正用导航参数误差的估值去校正系统输出的导航参数，得到综合导航系统的导航参数估值即：ˆIXXˆIIXXXˆˆ间接法卡尔曼滤波器：将惯性系统和其他的导航系统各自计算的导航参数进行比较，其差值就包含了惯导某些导航参数误差，即：INININXXXXXXXX一、卡尔曼滤波导航理论与技术教研室2020/5/109离散卡尔曼滤波方程1/)(kkkkkPHKIP或11,1/kkkkkXX状态一步预测方程)(1/1/kkkkkkkkXHZKXX状态估计计算方程11/1/)(kTkkkkTkkkkRHPHHPK滤波增益方程TkkkTkkkkkkkQPP1111,11,1/一步预测均方误差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/估计均方误差方程一、卡尔曼滤波导航理论与技术教研室2020/5/10101/kk11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/1ˆkX1kkkXˆ1kPkP1kk1,kkTkkkQ111kRkHkRkH1/kkPkKkZ滤波计算回路增益计算回路一、卡尔曼滤波导航理论与技术教研室2020/5/1011（1）状态一步预测方程1ˆkXXk-1的卡尔曼滤波估值1/ˆkkX利用计算得到的一步预测1ˆkX/11,1kkkkkXXkw的均值为零一、卡尔曼滤波导航理论与技术教研室2020/5/1012（2）状态估计方程计算估值Xk的方程。它是在一步预测Xk/k-1的基础上，根据量测值Zk计算出来的)(1/1/kkkkkkkkXHZKXXkkkkkkkkkkkkkkVXHXHVXHXHZ1/1/1/~上式就是通过计算新息，并左乘一个系数矩阵把估计出来，加到中，从而得到估值，称为滤波增益矩阵1/kkX1/~kkXkXˆkKkK一、卡尔曼滤波导航理论与技术教研室2020/5/1013（3）估计均方误差方程Kk选取的标准就是卡尔曼滤波的估计准则，也就是使得均方误差阵最小，为求Kk,先推导估计均方误差kXˆ|1ˆkkkkkkkkkxxxIKHxKVkTkkPExx|1|1|1kkTkkkkPExx|1|100kkkTTkkkExVEVx预测误差与量测噪声互不相关，故|1kkxkV1,00,10kkkkiiiixxW和与量测噪声互不相关。|1ˆkkxkxkV|1|1TkkkkkkkkkkkkkPEIKHxKVIKHxKV/1()()TTkkkkkkkkkkPIKHPIKHKRK一、卡尔曼滤波导航理论与技术教研室2020/5/1014（4）滤波增益方程11/1/)(kTkkkkTkkkkRHPHHPKKk选取的原则是使得均方误差阵最小；kP采用微分求极值的方法，即：将改为，得到的估计均方误差标以，其与的差值为，求出使为非负定的值就是使为最小的。kKkkKKkPkkkPPPkPkPkKkPkK/1()TTTkkkkkkkkPWWKHPHRK/1TTTkkkkkkkkWKHPIHKRK一、卡尔曼滤波导航理论与技术教研室2020/5/1015是用来估计的系数矩阵。,1,1ˆˆˆkkkkkkkkxxKZHx1,1,1TTkkkkkkkkkKPHHPHR,1,1kkkkkkPKPR,1kkZ,1kkxkK设状态矢量和量测矢量都是一维的，且H=1，这说明增益系数是预测均方误差在它和量测噪声方差之和中所占的比例。一、卡尔曼滤波增益矩阵的直观意义导航理论与技术教研室2020/5/1016（5）一步预测均方误差方程11/1/)(kTkkkkTkkkkRHPHHPK从下式可以看出，求Kk必须先求出Pk/k-1式中，为的估计误差，可以看出一步预测均方误差阵Pk/k-1是从估计均方误差阵Pk-1转移过来的，并且再加上系统噪声方差的影响。111ˆkkkXXX1ˆkXTkkkTkkkkkkkQPP1111,11,1/的均方误差阵，即：TkkkXXEP111~,~1ˆkX一、卡尔曼滤波导航理论与技术教研室2020/5/1017二、自适应卡尔曼滤波在卡尔曼滤波计算中，可能出现发散的现象。当量测值数目不断增加时，按滤波方程计算的估计均方误差阵趋于零或趋于某一稳态值，但估计值和实际状态的偏差却越来越大，使滤波器逐渐失去估计作用。这种现象称为滤波器的发散。导航理论与技术教研室2020/5/1018设系统和量测为：状态和量测都是标量，和为互不相关的零均值白噪声序列，并有：111,2,rrkkkrrkkkxxwzxvkrkxrkzkwkv1kkQR设，试求解状态估计值，估计误差方差以及真实误差。在设计滤波器时误认为系统没有噪声，即系统模型为：00ˆ0,1xP1kkxx二、自适应卡尔曼滤波滤波发散例子导航理论与技术教研室2020/5/1019二、自适应卡尔曼滤波计算过程中的舍入误差引起。系统的数学模型或噪声模型的统计特性不准确，没有能够真实的反映系统的真实过程。滤波发散原因导航理论与技术教研室2020/5/1020二、自适应卡尔曼滤波Sage-Husa自适应滤波强跟踪Kalman滤波改进的自适应滤波渐消记忆自适应滤波平方根滤波几种主要的自适应滤波方法导航理论与技术教研室2020/5/1021二、自适应卡尔曼滤波Sage-Husa自适应滤波利用观测数据进行递推滤波的同时，通过时变噪声统计估值器，实时估计和修正系统噪声和观测噪声的统计特性，从而达到降低模型误差、抑制滤波发散、提高滤波精度的目的。导航理论与技术教研室2020/5/1022二、自适应卡尔曼滤波1、利用标准的卡尔曼滤波方程计算出各个状态的估计值11,111,1,111,111,111,1,1,1111,111111111,ˆˆˆˆˆˆ[]ˆˆ[][]ˆˆkkkkkkkkkkkTTkkkkkkkkkTkkkkkkkkTTkkkkkkkkkkkkkkkkXXKZXXqKPHHPHRPPQPIKHPIKHKRKZZHXrSage-Husa自适应滤波导航理论与技术教研室2020/5/1023二、自适应卡尔曼滤波2、计算系统噪声和量测噪声的均值和方差矩阵111,1,11111,1111,1111111,1,ˆˆˆ(1)[]ˆˆ(1)[]ˆˆˆˆ(1)[]ˆ(1)[]kkkkkkkkkTTkkkkkkkkkkkkkkkkkkTTTkkkkkkkkkkkkkkrdrdZHXRdRdZZHPHqdqdXXQdQdKZZKPPSage-Husa自适应滤波导航理论与技术教研室2020/5/1024二、自适应卡尔曼滤波Sage-Husa自适应滤波实时解算系统噪声统计特性，计算精度较高；增加了计算量，实时性难以保证；计算噪声方差易失去正定性，稳定性和收敛性不能完全保证。导航理论与技术教研室2020/5/1025二、自适应卡尔曼滤波强跟踪Kalman滤波通过牺牲一定的精度换取滤波稳定性将状态估计一步预测均方误差阵乘以加权系数导航理论与技术教研室2020/5/1026二、自适应卡尔曼滤波1,11,1,TkkkkkkkkkPPQ111[(1),....,()]kkkdiagn111111,1,111[][]TkkkkkknTTikkkkkkkitrvRHQHCPHH11111(0)(1,01)1TTkkkkZZkvvZZk强跟踪Kalman滤波1111()1)()(1)1ikikkikCCiC状态发生突变kZ增大1,kkkkkvCP、、、增大跟踪状态变化导航理论与技术教研室2020/5/1027二、自适应卡尔曼滤波强跟踪Kalman滤波结构简单、跟踪能力强、可靠性高；破坏了滤波器的最优条件，滤波精度降低。导航理论与技术教研室2020/5/1028二、自适应卡尔曼滤波改进的自适应滤波滤波器是否收敛?开始赋初值k=1k大于采样时间结束k=k+1NNYY强跟踪Kalman滤波算法Sage-Husa自适应滤波算法导航理论与技术教研室2020/5/1029二、自适应卡尔曼滤波收敛性判据：Tr{[]}TTkkkkZZEZZ,1ˆkkkkkZZHX储备系数量测估计误差，即新息,1TkkkkkHPHR改进的自适应滤波导航理论与技术教研室2020/5/1030二、自适应卡尔曼滤波卡尔曼滤波器发散，计算的协方差阵P逐渐趋于零，状态估计过于依赖过去的数据，这种情况下，常采取渐消记忆的卡尔曼滤波来克服发散。采用渐消记忆限制Kalman滤波器对旧数据的利用权重，以便充分利用现时的观测数据。渐消记忆自适应滤波导航理论与技术教研室2020/5/1031二、自适应卡尔曼滤波一步预测估计误差方程|1,11,1111TTkkkkkkkkkkPsPQ渐消因子S1，使增大，进而增大，使得新量测值在估计值中的权重增加，“过老”的量测值的作用相对地减少