卡尔曼滤波器原理之基本思想续

Posted 2020-10-03 爱酷媒

　　在上一篇文章卡尔曼滤波器原理之基本思想中，我们分析并推导了基于卡尔曼一步预测的滤波器状态递推公式，接下来，我们将完成上一次的推导过程。首先，我们拿来上次的推导结果：

\\[\\hat x(n + 1|{{\\bf{Y}}_n}) = \\sum\\limits_{k = 1}^{n - 1} {E[x(n + 1){\\alpha ^H}(k)]{{\\bf{R}}^{ - 1}}(k)\\alpha (k)}  + E[x(n + 1){\\alpha ^H}(k)]{{\\bf{R}}^{ - 1}}(k)\\alpha (k)\\]

　　右侧的求和项由过程方程可改写为：

\\[\\begin{array}{l}
 \\sum\\limits_{k = 1}^{n - 1} {E[x(n + 1){\\alpha ^H}(k)]{{\\bf{R}}^{ - 1}}(k)\\alpha (k)}  \\\\
  = \\sum\\limits_{k = 1}^{n - 1} {E\\left\\{ {\\left[ {{\\bf{F}}(n + 1,n)x(n) + {v_1}} \\right]{\\alpha ^H}(k)} \\right\\}{{\\bf{R}}^{ - 1}}(k)\\alpha (k)}  \\\\
  = {\\bf{F}}(n + 1,n)\\sum\\limits_{k = 1}^{n - 1} {E\\left\\{ {\\left[ {x(n) + {v_1}} \\right]{\\alpha ^H}(k)} \\right\\}} {{\\bf{R}}^{ - 1}}(k)\\alpha (k) \\\\
  = {\\bf{F}}(n + 1,n)\\sum\\limits_{k = 1}^{n - 1} {E\\left[ {x(n){\\alpha ^H}(k)} \\right]} {{\\bf{R}}^{ - 1}}(k)\\alpha (k) \\\\
  = {\\bf{F}}(n + 1,n)\\hat x(n|{{\\bf{Y}}_{n - 1}}) \\\\
 \\end{array}\\]

　　同时，如果我们定义矩阵：

\\[{\\bf{G}}(n) = E[x(n + 1){\\alpha ^H}(n)]{{\\bf{R}}^{ - 1}}(n)\\]

　　这里G(n)就是大名鼎鼎的卡尔曼增益。结合上面两个式子，系统状态的递推公式可以简写为

\\[\\hat x(n + 1|{{\\bf{Y}}_n}) = {\\bf{F}}(n + 1,n)\\hat x(n|{{\\bf{Y}}_{n - 1}}) + {\\bf{G}}(n)\\alpha (n)\\]

 　　等式右侧第二项通常称为修正项，上式的物理意义是比较明确的，它表明：系统状态的最小均方估计可以由前一个估计再加上修正项得到。这样一来，观测值是已知的，说明新息是很容易求出来的，只要再有了卡尔曼增益G(n)，我们就可以由当前状态预估下一个状态，这样不断的迭代更新...，慢着！好像忽略了一个问题，那就是第一个系统状态估计，也就是初始值。其实这个好解决，通常设为系统第一个状态的经验均值就可以，只要卡尔曼预测过程是收敛的，就不会有问题。至此，我们唯一剩下的问题是：要以什么样的方式来计算卡尔曼增益G(n)

　　首先，我们计算

\\[E[x(n + 1){\\alpha ^H}(n)] = {\\bf{F}}(n + 1,n)E[x(n){\\alpha ^H}(n)] = {\\bf{F}}(n + 1,n)E[x(n){[{\\bf{C}}(n)\\varepsilon (n,n - 1) + {v_2}(n)]^H}]\\]

　　因为状态x(n)与噪声v₂(n)互不相关，所以

\\[E[x(n + 1){\\alpha ^H}(n)] = {\\bf{F}}(n + 1,n)E[x(n){\\varepsilon ^H}(n,n - 1)]{{\\bf{C}}^H}(n) = {\\bf{F}}(n + 1,n)E[(\\varepsilon (n,n - 1) + \\hat x(n|{{\\bf{Y}}_{n - 1}})){\\varepsilon ^H}(n,n - 1)]{{\\bf{C}}^H}(n)\\]

　　又因为状态误差向量与其估计正交，因此

\\[E[x(n + 1){\\alpha ^H}(n)] = {\\bf{F}}(n + 1,n)E[\\varepsilon (n,n - 1){\\varepsilon ^H}(n,n - 1)]{{\\bf{C}}^H}(n) = {\\bf{F}}(n + 1,n){\\bf{K}}(n,n - 1){{\\bf{C}}^H}(n)\\]

　　所以

\\[{\\bf{G}}(n) = E[x(n + 1){\\alpha ^H}(n)]{{\\bf{R}}^{ - 1}}(n) = {\\bf{F}}(n + 1,n){\\bf{K}}(n,n - 1){{\\bf{C}}^H}(n){{\\bf{R}}^{ - 1}}(n)\\]

　　看来，事情还没有完，又出现了一个新的变量，状态误差相关矩阵。看来，还是无法利索的得到卡尔曼增益。那么，我们试试看能不能导出状态误差相关矩阵的递归表示。这样，一切就都完美了！让我们开始吧，这一步耗死了我不少脑细胞。《自适应滤波器原理》这本书中详细的细节并没有给出，下面把详细过程给出来。先把状态误差展开：

\\[\\begin{array}{l}
 \\varepsilon (n + 1,n) = x(n + 1) - \\hat x(n + 1|{{\\bf{Y}}_n}) \\\\
  = {\\bf{F}}(n + 1,n)x(n) + {v_1} - \\left[ {{\\bf{F}}(n + 1,n)\\hat x(n|{{\\bf{Y}}_{n - 1}}) + {\\bf{G}}(n)\\alpha (n)} \\right] \\\\
  = {\\bf{F}}(n + 1,n)\\left[ {x(n) - \\hat x(n|{{\\bf{Y}}_{n - 1}})} \\right] - {\\bf{G}}(n)\\alpha (n) + {v_1}(n) \\\\
  = {\\bf{F}}(n + 1,n)\\left[ {x(n) - \\hat x(n|{{\\bf{Y}}_{n - 1}})} \\right] - {\\bf{G}}(n)\\left[ {y(n) - {\\bf{C}}(n)\\hat x(n|{{\\bf{Y}}_{n - 1}})} \\right] + {v_1} \\\\
  = {\\bf{F}}(n + 1,n)\\left[ {x(n) - \\hat x(n|{{\\bf{Y}}_{n - 1}})} \\right] - {\\bf{G}}(n)\\left[ {{\\bf{C}}(n)x(n) + {v_2}(n) - {\\bf{C}}(n)\\hat x(n|{{\\bf{Y}}_{n - 1}})} \\right] + {v_1}(n) \\\\
  = {\\bf{F}}(n + 1,n)\\left[ {x(n) - \\hat x(n|{{\\bf{Y}}_{n - 1}})} \\right] - {\\bf{G}}(n){\\bf{C}}(n)\\left[ {x(n) - \\hat x(n|{{\\bf{Y}}_{n - 1}})} \\right] + {\\bf{G}}(n){v_2}(n) + {v_1}(n) \\\\
  = \\left[ {{\\bf{F}}(n + 1,n) - {\\bf{G}}(n){\\bf{C}}(n)} \\right]\\varepsilon (n,n - 1) + {\\bf{G}}(n){v_2}(n) + {v_1}(n) \\\\
 \\end{array}\\]

　　现在我们可以以此为依据把状态误差相关矩阵来计算，看看能推出什么。因为展开的过程中式子会比较长，为了公式排版更好，先简写一下符号，定义

\\[\\begin{array}{l}
 {\\begin{array}{*{20}{c}}
   {{\\bf{K}}(n,n - 1) = {{\\bf{K}}_{n - 1}}} & {{\\bf{G}}(n) = {{\\bf{G}}_n}} & {{{\\bf{Q}}_2}(n) = {{\\bf{Q}}_2}} & {{{\\bf{Q}}_1}(n) = {\\bf{Q}}}  \\\\
\\end{array}_1} \\\\
 \\begin{array}{*{20}{c}}
   {{\\bf{C}}(n) = {{\\bf{C}}_n}} & {{{\\bf{C}}^H}(n) = {\\bf{C}}_n^H}  \\\\
\\end{array} \\\\
 \\begin{array}{*{20}{c}}
   {{\\bf{R}}(n) = {{\\bf{R}}_n}} & {{{\\bf{R}}^{ - 1}}(n) = {\\bf{R}}_n^{ - 1}}  \\\\
\\end{array} \\\\
 \\begin{array}{*{20}{c}}
   {{\\bf{F}}(n + 1,n) = {{\\bf{F}}_n}} & {{\\bf{F}}(n + 1,n){\\bf{F}}(n,n + 1)}  \\\\
\\end{array} = {\\bf{I}} \\\\
 \\end{array}\\]

　　好了，开始正式展开吧，状态误差相关矩阵展开可以表示为（以下推导过程中利用了状态误差相关矩阵与其共轭相等的性质）：

\\[\\begin{array}{l}
 {\\bf{K}}(n + 1,n) = E\\left[ {\\varepsilon (n + 1,n){\\varepsilon ^H}(n + 1,n)} \\right] \\\\
  = \\left[ {{\\bf{F}}(n + 1,n) - {\\bf{G}}(n){\\bf{C}}(n)} \\right]{\\bf{K}}(n,n - 1){\\left[ {{\\bf{F}}(n + 1,n) - {\\bf{G}}(n){\\bf{C}}(n)} \\right]^H} + {\\bf{G}}(n){{\\bf{Q}}_2}(n){{\\bf{G}}^H}(n) + {{\\bf{Q}}_1}(n) \\\\
  = [{{\\bf{F}}_n} - {{\\bf{G}}_n}{{\\bf{C}}_n}]{{\\bf{K}}_{n - 1}}{[{{\\bf{F}}_n} - {{\\bf{G}}_n}{{\\bf{C}}_n}]^H} + {{\\bf{G}}_n}{{\\bf{Q}}_2}{\\bf{G}}_n^H + {{\\bf{Q}}_1} \\\\
  = [{{\\bf{F}}_n} - {{\\bf{F}}_n}{{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}]{{\\bf{K}}_{n - 1}}{[{{\\bf{F}}_n} - {{\\bf{F}}_n}{{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}]^H} + {{\\bf{G}}_n}{{\\bf{Q}}_2}{\\bf{G}}_n^H + {{\\bf{Q}}_1} \\\\
  = {{\\bf{F}}_n}[{\\bf{I}} - {{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}]{{\\bf{K}}_{n - 1}}{[{\\bf{I}} - {{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}]^H}{\\bf{F}}_n^H + {{\\bf{G}}_n}{{\\bf{Q}}_2}{\\bf{G}}_n^H + {{\\bf{Q}}_1} \\\\
  = {{\\bf{F}}_n}\\left[ {{{\\bf{K}}_{n - 1}} - {{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{\\bf{K}}_{n - 1}^H - {{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{{\\bf{K}}_{n - 1}} + {{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{\\bf{K}}_{n - 1}^H} \\right]{\\bf{F}}_n^H + {{\\bf{G}}_n}{{\\bf{Q}}_2}{\\bf{G}}_n^H + {{\\bf{Q}}_1} \\\\
  = {{\\bf{F}}_n}\\left[ {{{\\bf{K}}_{n - 1}} - 2{{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{{\\bf{K}}_{n - 1}} + {{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}\\left( {{{\\bf{R}}_n} - {{\\bf{Q}}_2}} \\right){\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{\\bf{K}}_{n - 1}^H} \\right]{\\bf{F}}_n^H + {{\\bf{G}}_n}{{\\bf{Q}}_2}{\\bf{G}}_n^H + {{\\bf{Q}}_1} \\\\
  = {{\\bf{F}}_n}\\left[ {{{\\bf{K}}_{n - 1}} - 2{{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{{\\bf{K}}_{n - 1}} + {{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{R}}_n}{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{\\bf{K}}_{n - 1}^H - {{\\bf{F}}_{n + 1}}{{\\bf{G}}_n}{{\\bf{Q}}_2}{\\bf{G}}_n^H{\\bf{F}}_{n + 1}^H} \\right]{\\bf{F}}_n^H + {{\\bf{G}}_n}{{\\bf{Q}}_2}{\\bf{G}}_n^H + {{\\bf{Q}}_1} \\\\
  = {{\\bf{F}}_n}\\left[ {{{\\bf{K}}_{n - 1}} - 2{{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{{\\bf{K}}_{n - 1}} + {{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{\\bf{K}}_{n - 1}^H} \\right]{\\bf{F}}_n^H - {{\\bf{G}}_n}{{\\bf{Q}}_2}{\\bf{G}}_n^H + {{\\bf{G}}_n}{{\\bf{Q}}_2}{\\bf{G}}_n^H + {{\\bf{Q}}_1} \\\\
  = {{\\bf{F}}_n}\\left[ {{{\\bf{K}}_{n - 1}} - {{\\bf{K}}_{n - 1}}{\\bf{C}}_n^H{\\bf{R}}_n^{ - 1}{{\\bf{C}}_n}{{\\bf{K}}_{n - 1}}} \\right]{\\bf{F}}_n^H + {{\\bf{Q}}_1} \\\\
  = {{\\bf{F}}_n}\\left[ {{{\\bf{K}}_{n - 1}} - {{\\bf{F}}_{n + 1}}{{\\bf{G}}_n}{{\\bf{C}}_n}{{\\bf{K}}_{n - 1}}} \\right]{\\bf{F}}_n^H + {{\\bf{Q}}_1} \\\\
  = {\\bf{F}}(n + 1,n)\\left[ {{\\bf{K}}(n,n - 1) - {\\bf{F}}(n,n + 1){\\bf{G}}(n){\\bf{C}}(n){\\bf{K}}(n,n - 1)} \\right]{{\\bf{F}}^H}(n + 1,n) + {{\\bf{Q}}_1}(n) \\\\
  \\Rightarrow \\left\\{ {\\begin{array}{*{20}{c}}
   {{\\bf{K}}(n + 1,n) = {\\bf{F}}(n + 1,n){\\bf{K}}(n){{\\bf{F}}^H}(n + 1,n) + {{\\bf{Q}}_1}(n)} \\hfill  \\\\
   {{\\bf{K}}(n) = {\\bf{K}}(n,n - 1) - {\\bf{F}}(n,n + 1){\\bf{G}}(n){\\bf{C}}(n){\\bf{K}}(n,n - 1)} \\hfill  \\\\
\\end{array}} \\right. \\\\
 \\end{array}\\]

　　上述就是著名的Riccati递归方程。不容易啊，至此，终于完成了线性卡尔曼滤波的推导过程。总结以上过程，线性卡尔曼滤波器主要步骤表示如下：

\\[\\begin{array}{l}
 {\\bf{G}}(n) = {\\bf{F}}(n + 1,n){\\bf{K}}(n,n - 1){{\\bf{C}}^H}(n){[{\\bf{C}}(n){\\bf{K}}(n,n - 1){{\\bf{C}}^H}(n) + {{\\bf{Q}}_2}(n)]^{ - 1}} \\\\
 \\alpha (n) = y(n) - {\\bf{C}}(n)\\hat x(n|{{\\bf{y}}_{n - 1}}) \\\\
 \\hat x(n + 1|{{\\bf{y}}_n}) = {\\bf{F}}(n + 1,n)\\hat x(n|{{\\bf{y}}_{n - 1}}) + {\\bf{G}}(n)\\alpha (n) \\\\
 {\\bf{K}}(n) = {\\bf{K}}(n,n - 1) - {\\bf{F}}(n,n + 1){\\bf{G}}(n){\\bf{C}}(n){\\bf{K}}(n,n - 1) \\\\
 {\\bf{K}}(n + 1,n) = {\\bf{F}}(n + 1,n){\\bf{K}}(n){{\\bf{F}}^H}(n + 1,n) + {{\\bf{Q}}_1}(n) \\\\
 \\end{array}\\]

　　现在，再回想这两篇博客的分析过程，不难看出，卡尔曼滤波器通过使用过程方程对系统的行为进行建模，再通过测量方程对观测行为进行建模。尽可能利用测量信息来得到准确的状态，也是说卡尔曼滤波器的核心是如何求解过程方程和测量方程，难点在于理解均值和协方差的传播过程。再往大了说，它教给了我们一整套如何去认识系统、观察分析系统的方法。可能分析到这里，可能很多朋友觉得终于理解了这个重要的滤波器。可事情并没有这么简单，应该还有以下疑问需要进一步的解决，不解决掉，很可能没有真正理解透彻。

　　首先是过程方程，可以看到，过程方程对系统的建模是，系统的下一个状态只与前一个状态有关。可能在有的系统中是可以的，但不够通用化，也不符合很多情况。实际中最常见到的情景是系统的当状态不只应该与前面的状态有关，还与当前的输入有关，这方面可以去看下Lyapunov方程。

　　其次，以上推导过程只是前人把理论摸透了，后人整理规整出来的，并不是正常思维下的人们解决问题的思考方法。也就是说，这里并没有解决滤波器的发明者是如何想到用这个方法去解决碰到的问题

　　再次，卡尔曼滤波器还有很多变种，这些变种是出于什么目的，为了解决什么问题被发明出来，优劣如何，最好也要搞清楚。

　　最后，这里理论分析太多了，没有给出一个或几个实际例子，真碰到问题，可能还是不会灵活的使用，甚至不会用。这几个问题先留着吧，后面逐步进行解答，不管怎么样，先把基本思想理解了。也是一件值得开心的事呢！