卡尔曼滤波与组合导航-第三讲

卡尔曼滤波与组合导航TheoryofKalmanfilterandIntegratedNavigation2第三章卡尔曼滤波原理3.1卡尔曼滤波与最优估计3.2卡尔曼滤波方程3.3连续系统的卡尔曼滤波方程3.4连续—离散系统卡尔曼滤波方程3.5卡尔曼滤波在组合导航中的应用方式3.6非线性系统的卡尔曼滤波3.7卡尔曼滤波与系统可观测性分析33.1卡尔曼滤波与最优估计卡尔曼滤波是一种最优估计技术！它能将仅与部分状态有关的测量值进行处理，得出从某种统计意义上讲估计误差最小的更多的状态的估计值。估计误差最小的标准称为估计准则。根据不同的估计准则和估计计算方法，有各种不同的最优估计。卡尔曼滤波是一种递推线性最小方差估计。最小方差估计线性最小方差估计递推线性最小方差估计41、最小方差估计最小方差估计的估计准则是估计的均方误差最小，即：})]()][({[})]()][({[TTZXZXEZXXZXXE系统的n维随机向量Z是m维随机量测向量利用Z计算得到的X的最小方差估值估计的误差估计均方差阵根据其他方法用Z计算得到的X的估值最小方差估计的误差小于等于其他估计的均方误差!5估计的均方误差就是估计误差的方差，即：0~)]}({[XEZXXETTXEXXEXEXXE]~~][~~[~~最小方差估计具有无偏性质，即它的估计误差（亦可用表示）的均值为零。即：X~因此，最小方差估计不但使估值的均方误差最小，而且这种最小的均方误差就是估计的误差方差)(ZX62、线性最小方差估计如果将估值规定为量测矢量Z的线性函数，即X式中A和b分别是（n×m）阶和n维的矩阵和矢量。这样的估计方法称为线性最小方差估计。可证明，这种估计只需要被估计值X和量测值Z的一、二阶统计特性，所以，它比最小方差估计较为实用。bAZX73、递推线性最小方差估计——卡尔曼滤波卡尔曼滤波的准则与线性最小方差估计相同估值同样是量测值的线性函数只要包括初始值在内的滤波器初值选择正确，它的估值也是无偏的})]()][({[})]()][({[TTZXZXEZXXZXXEbAZX0~)]}({[XEZXXE计算方法——递推形式8在k时刻以前估值的基础上，根据k时刻的量测值Zk,递推得到k时刻的状态估计值：根据k-1时刻以前所有的量测值得到1kXkZkX)(ˆtXX（k）也可以说是综合利用k时刻以前的所有量测值得到的一次仅处理一个量测量计算量大大减小主要适用于线性动态系统！93.2卡尔曼滤波方程1、离散系统的数学描述设离散化后的系统状态方程和量测方程分别为：kkkkkkkkkkVXHZWXX1111,Xk为k时刻的n维状态向量（被估计量）Zk为k时刻的m维量测向量k-1到k时刻的系统一步状态转移矩阵（n×n阶）Wk-1为k-1时刻的系统噪声（r维）Γk-1为系统噪声矩阵（n×r阶）Hk为k时刻系统量测矩阵（m×n阶）Vk为k时刻m维量测噪声10Qk和Rk分别称为系统噪声和量测噪声的方差矩阵，在卡尔曼滤波中要求它们分别是已知值的非负定阵和正定阵；δkj是Kroneckerδ函数，即：)(1)(0jkjkkj卡尔曼滤波要求{Wk}和{Vk}是互不相关的零均值的白噪声序列，有：kjkTjkkjkTjkRVVEQWWE11Var{·}为对{·}求方差的符号卡尔曼滤波要求mx0和Cx0为已知量，初始状态的一、二阶统计特性为：00xmXE00xCXVar且要求X0与{Wk}和{Vk}都不相关122、离散卡尔曼滤波方程1/)(kkkkkPHKIP或11,1/kkkkkXX状态一步预测方程)(1/1/kkkkkkkkXHZKXX状态估值计算方程11/1/)(kTkkkkTkkkkRHPHHPK滤波增益方程TkkkTkkkkkkkQPP1111,11,1/一步预测均方差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/估计均方差方程132、离散卡尔曼滤波方程11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/时间修正方程量测修正方程14（1）状态一步预测方程各滤波方程的物理意义：1ˆkXXk-1的卡尔曼滤波估值1/ˆkkX利用Xk-1计算得到的一步预测也可以认为是利用k-1时刻和以前时刻的量测值得到的Xk的一步预测15上式就是通过计算新息，把估计出来，并左乘一个系数矩阵加到中，从而得到估值和，称为滤波增益矩阵1/kkX1/~kkXkXˆkKkK（2）状态估值计算方程计算估值Xk的方程。它是在一步预测Xk/k-1的基础上，根据量测值Zk计算出来的)(1/1/kkkkkkkkXHZKXXkkkkkkkkkkkkkkVXHXHVXHXHZ1/1/1/~一步预测误差1/1/~kkkkkXXX若把看作是量测的一步预测，则就是量测的一步预测误差1/kkkXH)(1/kkkkXHZkZ由两部分组成：和，正是在基础上估计所需信息，因此又称为新息1/kkXkV1/~kkX1/~kkXkX)(1/kkkkXHZ16由于也具有无偏性，即的均值为零，所以也称为一步预测误差方差阵。上式中的和分别就是新息中的两部分内容1/ˆkkX1/~kkX1/kkPTkkkkHPH1/kR（3）滤波增益方程一步预测均方差阵，即：/1/1/1,TkkkkkkkkZHPEXX11/1/)(kTkkkkTkkkkRHPHHPKKk选取的标准就是卡尔曼滤波的估计准则，也就是使得均方误差阵最小：kXˆ如果Rk大，Kk就小Rk小，Kk就大17（4）一步预测均方误差方程11/1/)(kTkkkkTkkkkRHPHHPK从下式可以看出，求Kk必须先求出Pk/k-1式中，为的估计误差，可以看出一步预测均方误差阵Pk/k-1是从估计均方误差阵Pk-1转移过来的，并且再加上系统噪声方差的影响。111ˆ~kkkXXX1ˆkXTkkkTkkkkkkkQPP1111,11,1/的均方误差阵，即：TkkkXXEP111~,~1ˆkX18（5）估计均方误差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/1/)(kkkkkPHKIP或计算量小，但在计算机有舍入误差的条件下，不能始终保证算出的Pk是对称的19（6）卡尔曼滤波的计算流程1/kk11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/1ˆkX1kkkXˆ1kPkP1kk1,kkTkkkQ111kRkHkRkH1/kkPkKkZ滤波计算回路增益计算回路20（7）初值的确定在滤波开始时，必须有初始值和才能进行0ˆX0P为了保证估值的无偏性，应选择：000ˆxmXEXTxxTmXmXEXXXXEP))(()ˆ)(ˆ(00000000000xCXVar这样才能保证估计均方差阵Pk始终最小。21另外，如果系统和量测值中都有已知确定输入量即系统的状态方程和量测方程为一步预测方程改为：,111111kkkkkkkkkkkkkXXWBUZHXVY状态估计方程改为：1111,ˆˆ1kkkkkUBXXkk)ˆ(ˆˆ11kkkkXHYZKXXkkkkk其他滤波方程不变223.3连续系统的卡尔曼滤波方程一般情况下经常采用离散卡尔曼滤波方程但也可以直接从连续系统的滤波方程求出系统状态的估值，设连续系统的状态方程和量测方程分别为：)()()()()(tWtGtXtFtX)()()()(tVtXtHtZX(t)和Z(t)分别为t时刻的系统状态矢量和量测矢量；F(t)，G(t)和H(t)，分别为系统矩阵，系统噪声矩阵和量测矩阵；W(t)和V(t)和分别为系统噪声矢量和量测噪声矢量，卡尔曼滤波要求它们都是零均值的白噪声过程。23根据估计均方误差最小的估计准则，按上述系统和量测值，可以推导出连续系统的滤波方程，即R-1(t)为是量测噪声的方差强度阵R(t)的逆阵，即Q(t)是系统噪声的方差强度阵，即)](ˆ)()()[()(ˆ)()(ˆtXtHtZtKtXtFtX)()()()(1tRtHtPtKT)()()()()()()()()()()()()(1tGtQtGtPtHtRtHtPtPtFtFtPtPTTT)()()}()({ttRVtVET)()()()(})]()()][()({[ttGtQtGWGtWtGETT243.4连续—离散系统卡尔曼滤波方程实际被估计状态的系统经常是连续系统，而量测是间隔时间的，这种被估计对象常称为连续—离散系统如考虑系统有确定性输入量，则系统方程和量测方程分别：)()()()()(tWtGtXtFtXkkkkVXHZ25滤波方程:量测修正方程)()(ˆ)()(ˆtUtXtFtX)()()()()()()()(tGtQtGtFtPtPtFtPTT)(ˆˆ1ktXXkk)(1ktPPkk]ˆ[ˆˆ11kkjkkXHZKXXkkkk111()kkkkTTkkkkkKPHHPHR11)()()(kkkkPHKIKRKHKIPHKIPkkTkkkTkkkkk时间修正方程:可以采用微分方程的数值解法来求解，也可以用离散化的方法求解26连续系统离散化的实质①根据连续系统的系统矩阵F(t)计算出离散系统的转移矩阵ΦK/K-1②根据连续系统的系统噪声方差强度阵Q(t)计算出离散系统噪声方差阵QkΦK/K-1的计算方法如果计算周期T远小于系统阵F(t)发生明显变化所需要的时间，则ΦK/K-1可以利用定常系统的计算方法，即27)(!10)(1/knnnTtFkktFnTen上式中T为滤波器的计算周期TNT所以112111/)(iiiikkFTITOFTI28QK的计算方法滤波计算中需要的系统噪声方差形式为tkkkQ一般不单独计算QK)(Q(t))(tGtGT连续系统的通常设系统为定常系统，令tkkkQkQ=)(Q(t))(tGtGTQ=则......!3})]([])([{!2])([32TFQQFFQFQFFTQFQFTQQTTTTk计算时项数的确定与计算ΦK/K-1时的项数确定方法相同29时变系数也可按各分阶段间隔（△t）的系统阵为定常阵的假设来计算：如果计算周期T短，则也可按以下公式计算：21111)(TiFQQiFTQNQNiTiNiik2)(,1,1TQQQTkkkkk30有色噪声条件下的卡尔曼滤波卡尔曼滤波要求系统噪声和量测噪声必须是白噪声如果系统噪声和量测噪声是有色噪声，则在滤波估计之前，必须把有色噪声描述成以下一阶马尔科夫过程：量测噪声为有色噪声时，可扩充为状态变量kkkkkNN