卡尔曼滤波算法(含详细推导)

卡尔曼滤波算法及推导1、kalman滤波问题考虑一离散时间的动态系统，它由描述状态向量的过程方程和描述观测向量的观测方程共同表示。（1）、过程方程式中，M1向量x(n)表示系统在离散时间n的状态向量，它是不可观测的；MM矩阵F（n+1,n）成为状态转移矩阵，描述动态系统在时间n的状态到n+1的状态之间的转移，应为已知。而M1向量为过程噪声向量，它描述状态转移中间的加性噪声或误差。)1.......(),1(11)()()(nvnxnnFnx)(nv11、kalman滤波问题（1）、观测方程式中，N1向量y(n)表示动态系统在时间n的观测向量；NM矩阵C（n）称为观测矩阵（描述状态经过其作用，变成可预测的），要求也是已知的；v2(n)表示观测噪声向量，其维数与观测向量的相同。过程方程也称为状态方程，为了分析的方便，通常假定过程噪声v1(n)和观测噪声v2(n)均为零均值的白噪声过程，它们的相关矩阵分别为：)2.........()(2)()()(nvnxnCny1、kalman滤波问题)3......()}()({),(,0111knnQknHkvnvE)4......()}()({),(,0222knnQknHkvnvE1、kalman滤波问题还假设状态的初始值x(0)与v1(n)、v2(n)，n0均不相关，并且噪声向量v1(n)与v2(n)也不相关，既有：)5......(,,0)}()({21knkvnvEH2、新息过程考虑一步预测问题，给定观测值y(1),...，y(n-1)，求观测向量y(n)的最小二乘估计，记作（1）、新息过程的性质y(n)的新息过程定义为：式中，N1向量表示观测数据y(n)的新的信息，简称新息。))1(),...,1((ˆ)(ˆ1nyynynydef)6.().........(ˆ)()(1nynyn)(n2、新息过程新息具有以下性质：性质1n时刻的新息与所有过去的观测数据y(1),...，y(n-1)正交，即：性质2新息过程由彼此正交的随机向量序列{}组成，即有)(n)(n)7.......(11,0)}()({nkkynEH)8(..........11,0)}()({nkknEH)(n2、新息过程性质3表示观测数据的随机向量序列{y(1),…y(n)}与表示新息过程的随机向量序列{a(1),…a(n)}一一对应，即以上性质表明：n时刻的新息a(n)是一个与n上课之前的观测数据y(1),...，y(n-1)不相关，并具有白噪声性质的随机过程，但它却能够提供有关y(n)的新息，这就上信息的内在物理含义。)9..()}........(),...1({)}(),...1({nnyy2、新息过程（2）、新息过程的计算下面分析新息过程的相关矩阵在kalman滤波中，并不直接估计观测数据向量的进一步预测，而是先计算状态向量的一步预测然后再用到下式得到：)11())........1(),...1(()(1nyynxndefx)10.()}........()({)(nnEnRH)(1ny)12..().........()()(11nxnCny2、新息过程将上式代入新息过程的定义式（6），可得到：这就是新息过程的实际计算公式，条件是：一步预测的状态向量估计业已求出。定义向量的一步预测误差：)14.().........()(),1(1nxnxnnedef)13.().........()]()()[()()()()(211nvnxnxnCnxnCnyn)(1nx2、新息过程将此式代入式（13），则有在新息过程的相关矩阵定义式（10）中代入式（14），并注意到观测矩阵C（n）是一已知的确定矩阵，故有式中Q2(n)是观测噪声v2(n)的相关矩阵，而表示（一步）预测状态误差的相关矩阵)15().........()1,()()(2nvnnenCn)16...(....................).........()()1,()()}()({)()}1,()1,({)()(222nQnCnnKnCnvnvEnCnnenneEnCnRHHHH)17...(....................)}........1,()1,({)1,(nnenneEnnKH3、kalman滤波算法由上一节的的新息过程的相关知识和信息后，即可转入kalman滤波算法的核心问题的讨论：如何利用新息过程估计状态向量的预测？最自然的方法是用新息过程序列a(1),…a(n)的线性组合直接构造状态向量的一布预测：式中W1(k)表示与一步预测项对应的权矩阵，且k为离散时间。现在的问题是如何确定这个权矩阵？（1）、状态向量的一布预测根据正交性原理，最优预测的估计误差nkdefkkWnyynnxx111)()())(),...,1(1()()1()1(n)1,e(n1nxnx3、kalman滤波算法应该与已知值正交，故有将式（18）代入（19），并利用新息过程的正交性，得到由此可以求出权矩阵的表达式：)20...().........()}()1({)(11KRknxEkWH)()()}()({)()}()1({11kRkWkkEkWknxEHH)19.........(,...,1,0)}()1()1({[)}(),1({1nkknxnxEknneEHH3、kalman滤波算法将式（20）代入式（18），状态向量的一步预测的最小均方估计可表示为注意到并利用状态方程(1)，易知下式对k=0，1，…，n成立：)21.....().........()()}()1({)()(})()1({)()(})()1({)1(1111111nnRnnxEkkRknxEkkRknxEnHnkHnkHx,,...,1,0,0)}()({1nkknvE3、kalman滤波算法将式（22）代入式（21）右边第一项（求和项），可将其化简为：)22.()}........()({),1()}()()(),1({[)}()1({1knxEnnFknvnxnnFEknxEHH)23....(..............................).........(),1()()(})()({),1()()(})()1({111111nxnnFkkRknxEnnFkkRknxEnkHnkH3、kalman滤波算法若定义并将式（23）和式（24）代入式（21），则得到状态向量一步预测的更新公式：式（25）在kalman滤波算法中起着关键的作用，因为它表明，n+1时刻的状态向量的一步预测分为非自适应（即确定）部分和自适应（即校正）部分G(n)a(n)。从这个意义上讲，G(n)称为kalman增益（矩阵）是合适的。)()}()1({)(1kRknxEnGHdef)25.....().........()()(),1()1(nnGnxnnFnx)(),1(nxnnF3、kalman滤波算法（2）、kalman增益的计算为了完成kalman自适应滤波算法，需要进一步推导kalman增益的实际计算公式。由定义式（24）知，只需要推导期望项的具体计算公式即可。将新息过程的计算公式（13）代入式（22），不难得出：这里使用了状态向量与观测噪声不相关的事实。进一步地，由正交原理引理知，在最小均方误差准则下求得的一步预测估与预测误差e(n,n-1)彼此正交，即)}()1({knxEH)(1nx)26)........(()}1,()({),1(})]()1,()()[({),1()}()({),1()()1({2nCnnenxEnnFnvnnenCnxEnnFnnxEnnFnnxEHHHHH0)}1,()({1NNenxEH3、kalman滤波算法因此，由式(26)及式(27)易得：将式(27)代入式(24)，便得到kalman增益的计算公式如下：式中R(n)是信息过程的相关矩阵，由式(10)定义。)28...().........()()1,(),1()(1nRnCnnKnnFnGH)27)........(()1,(),1()()}1,()1,({),1()()}1,()]1,()({[),1()}()1({nCnnKnnFnCnnenneEnnFnCnnennenxEnnFnnxEHHHHHH3、kalman滤波算法（3）、Riccati方程由式(28)表示的kalman增益与预测状态误差的相关矩阵K(n,n-1)有关，为了最后完成kalman自适应滤波算法，还需要再推导K(n,n-1)的递推公式。考察状态向量的预测误差：将状态方程(1)和状态向量的一步预测更新公式(25)代入式(29)中，有：将观测方程(2)代入上式，并代入，则有：)()(1)-ne(n,1nxnx)29..().........1()1(n)1,e(n1nxnx)()]()()()[()]()()[,1(n)1,e(n111nvnxnCnynGnxnxnnF)30.........().........()()(1)ne(n,)]()(),1([n)1,e(n21nvnGnvnCnGnnF3、kalman滤波算法求式(3)所示状态向量的一步预测误差向量的相关矩阵，容易证明：式中使用了e(n+1,n)，v1(n)，v2(n)彼此不相关的事实，以及和等关系式。对式(31)的右边进行展开，然后代入式(28)和(29)，可以证明：状态向量预测误差的相关矩阵的递推公式为：式中式(32)称为Riccati差分方程。)32.().........(),1()(),1(),1(1nQnnFkPnnFnnKH)33)........(1,()()(),1()1,()(1nnKnCnGnnFnnKnP)31.....().........()()()()]()(),1()[1,()]()(),1([)},1()],1({),1(21nGnQnGnQnCnGnnFnnKnCnGnnFnnenneEnnKHHH)()}()({111nQnvnvEH)()}()({222nQnvnvEH3、kalman滤波算法若定义是利用已知的y(1),…,y(n)求得的状态向量的滤波估计，则定义滤波状态向量的误差向量，可以证明：因此，Riccati差分方程中的矩阵P(n)事实上是滤波误差状态向量的相关矩阵。（4）、kalman滤波算法将上面推导得到的式(28)、(16)、(13)、(25)、(33)和(32)依次加以归纳，得到基于一步预测的kalman自适应滤波算法如下。初始条件：)(nx)}1({)1(},)]1()1()][1()1({[)0,1()}1({)1(1xExxxxxEKxExH其中)35.......(.....................................(n)}e(n)e{P(n)HE)34.......(..............................).........()(e(n)1nxnx3、kalman滤波算法输入观测向量过程：观测向量={y(1),…,y(n)}已知参数：状态转移矩阵F(n+1,n)观测矩阵C(n)过程噪声向量的相关矩阵Q1(n)观测噪声向量的相关矩阵Q2(n)计算：n=1,2,3,…)34.......(..............................)