【滤波】卡尔曼滤波数学

%matplotlib inline

#format the book
import book_format
book_format.set_style()

简介

如果你已经学习到了这一步，我希望你会认为卡尔曼滤波器有些名不副实。当然，我所书写的一些方程式，我希望对你来说是相当简单的。卡尔曼滤波的基本概念非常简单——进行两次观测，或者一次预测一次观测，然后选择介于两者之间的输出。如果你越相信观测值，你的估计值就越接近观测值；如果你越相信预测值，你的估计值就越接近预测值。

老实说，我一直在仔细地选择我所讨论的问题。对于一个任意的问题，设计卡尔曼滤波可能是非常困难的。不过，我并没有胡乱地选择。像牛顿运动方程这样的，在卡尔曼滤波应用中可以很容易地计算出来，它们构成了我们要解决的大部分问题。

之前的章节，我主要用代码和推理来说明这些概念，而非数学。但如果需要加深理解确实需要更多的数学知识。本文就是介绍这些。

动态系统建模

动态系统是一个物理系统，其状态量（位置、温度等）随时间而变化。微积分是改变数值的数学，所以我们用微分方程来模拟动态系统。有些系统不能用微分方程来建模，但我不会选择这种问题来研究。

建立动态系统模型是几门大学课程的内容。在某种程度上，没有什么可以取代几个学期的常微分方程和偏微分方程，还有控制系统理论的研究生课程。但如果你只是一个业余爱好者，或者在工作中试图解决一个非常具体的滤波问题，你可能没有太多的时间来进行这方面的钻研。

幸运的是，我将提供足够多的理论，使我们能够创建许多不同的卡尔曼滤波器的系统方程。我的目标是让你进入一个阶段，在这个阶段中，你可以独立阅读某些论文，并充分理解它来实现算法。尽管数学背景很深，但在实践中我们仅需要使用一些简单的技巧。

我们需要从了解卡尔曼滤波器使用的基本方程和假设开始。我们试图模拟现实世界的现象，那么我们要考虑什么呢？

每个物理系统都有一个过程。例如：一辆以一定速度行驶的汽车在一段固定的时间内行驶了一段距离，它的速度随加速度的变化而变化。我们用高中时期都学过的牛顿方程来描述这种行为。

$$v=at$$

$$x=\frac{1}{2}a{t}^2+v_{0}t+x_0$$

当我们学习微积分时，我们看到的它们是这样的：

$$v=\frac{dx}{dt}$$

$$a=\frac{dv}{dt}=\frac{d^{2}x}{d{t}^2}$$

一个典型的汽车跟踪问题：恒定速度或加速度下行驶，就像我们在前面章节所做的那样。但是，没有汽车能在完美的道路上行驶。颠簸、风阻、丘陵等情况可能会提高或降低速度。汽车的悬架是一个带有摩擦和不完美弹簧的机械系统。

完美地建模一个系统是不可能的，因此我们被迫做一个简化。在任何时候$t$，我们都说真实状态（比如我们的汽车的位置）是不完美系统模型的预测值加上一些未知的过程噪声：

$$x(t)=x_{pred}(t)+noise(t)$$

这并不意味着noise(t)是我们可以解析得出的函数。这仅仅是一个事实陈述——我们总是可以用预测值加上过程噪声来描述真实值。噪音并不意味着随机事件。如果我们在大气中追踪一个抛出的球，并且我们的模型假设球在真空中，那么在这种情况下，空气阻力的影响就是过程噪声。

在下文中，我们将学习如何将一组高阶微分方程转换为一组一阶微分方程。转换后，无噪声的系统模型为：

$$\dot{\mathbf{x}}=\mathbf{Ax}$$

$\mathbf{A}$被称为系统动力学矩阵，因为它描述了系统的动力学。现在我们需要模拟噪音，我们称之为$\mathbf{w}$，把它加到方程中。

$$\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{w}$$

你可能会觉得$\mathbf{w}$这个名字不合适，但你很快就会看到，卡尔曼滤波器假定白噪声（white）。

最后，我们需要考虑系统中的任何控制输入。我们假设一个输入$\mathbf{u}$，并且存在一个线性模型来定义该输入是如何影响系统的。例如，在你的车里踩油门会使它加速，重力会使球落下。两者都是控制输入。我们需要一个矩阵$\mathbf{B}$来将$\mathbf{u}$转换成对系统的影响。我们把它加到等式中：

$$\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{Bu}+\mathbf{w}$$

就这样。这是卡尔曼博士开始求解的方程之一，如果我们假设$\mathbf{w}$满足了某些性质（正态分布），他就发现了一个最优估计量。

动态系统的状态空间表示

我们已经推导出了这个方程：

$$\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{Bu}+\mathbf{w}$$

然而，我们对$\mathbf{x}$的导数不感兴趣，而是对$\mathbf{x}$本身感兴趣。暂时忽略噪声，我们需要一个方程，用时间$t_{k-1}$的$\mathbf{x}$来递归地找到时间$t_k$的$\mathbf{x}$值：

$$\mathbf{x}(t_k)=\mathbf{F}(\Delta t)\mathbf{x}(t_{k-1})+\mathbf{B}(t_k)\mathbf{u}(t_k)$$

我们约定将$\mathbf{x}(t_k)$写为${\mathbf{x}}_k$，这意味着第$k$个$\mathbf{x}$的值：

$${\mathbf{x}}_k=\mathbf{F}{\mathbf{x}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k$$

$\mathbf{F}$就是状态转移矩阵，因其在离散时间步长之间对状态量进行转移而得名。它与系统动力学矩阵$\mathbf{A}$非常相似。不同的是，$\mathbf{A}$建立了一组线性微分方程，并且是连续的。$\mathbf{F}$是离散的，表示一组线性方程（不是微分方程），在离散时间步长$\Delta t$上，将${\mathbf{x}}_{k-1}$转换为${\mathbf{x}}_k$。

找到$\mathbf{F}$这个矩阵通常是相当困难的。方程$\dot{x}=v$是最简单的微分方程，我们将其简单地积分为：

$${\int }_{x_{k-1}}^{x_k}dx={\int }_0^{\Delta t}vdt$$

$$x_k-x_0=v\Delta t$$

$$x_k=v\Delta t+x_0$$

这个方程是递归的：我们根据时间$t-1$的$\mathbf{x}$值计算时间$t$的$\mathbf{x}$值。这种递归形式使我们能够用卡尔曼滤波器要求的形式表示系统（过程模型）：

$${\mathbf{x}}_k=\mathbf{F}{\mathbf{x}}_{k-1}=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{k-1}\\ {\dot{x}}_{k-1}\end{array}\right]$$

我们可以这样做，是因为$\dot{x}=v$是最简单的微分方程。几乎所有其他的物理系统都会产生更复杂的微分方程，而这些微分方程都不一定可以使用这种方法。

状态空间方法在阿波罗任务后流行起来，这主要归功于卡尔曼博士的工作。这个想法很简单：用一个$n$阶微分方程建立系统模型，把它转换成一组等价的一阶微分方程组。即把它们转换成向量矩阵形式：$\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{Bu}$。在这种形式下，我们可以使用多种技术将这些线性微分方程转换为递归方程：

$${\mathbf{x}}_k=\mathbf{F}{\mathbf{x}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k$$

有些书把状态转移矩阵称为基本矩阵，许多人用$\mathbf{\Phi }$代替$\mathbf{F}$。

这些被称为状态空间方法，因为我们用系统状态来表示微分方程的解。

由高阶方程构造一阶方程组

许多物理系统模型需要控制输入为$\mathbf{u}$的二阶或更高阶微分方程：

$$a_n\frac{d^{n}y}{d{t}^n}+a_{n-1}\frac{d^{n-1}y}{d{t}^{n-1}}+...+a_2\frac{d^{2}y}{d{t}^2}+a_1\frac{dy}{dt}+a_0=u$$

状态空间方法需要一组等价的一阶微分方程组。通过定义导数的额外变量，任何高阶微分方程都可以简化为一阶的微分方程组。

我们来举个例子。给定系统$\ddot{x}-6\dot{x}+9x=u$，求等价的一阶方程。为了清楚起见，我使用了时间导数的点符号。

第一步是把最高阶项分离到方程的一边。

$$\ddot{x}=6\dot{x}-9x+u$$

我们定义了两个新变量：

$$x_1(u)=x$$

$$x_2(u)=\dot{x}$$

现在我们将这些代入原始方程并求解。这个解根据这些新变量得到一组一阶方程。为了便于记法，通常会去掉$(u)$。

我们知道${\dot{x}}_1=x_2$，${\dot{x}}_2=\ddot{x}$。因此：

$${\dot{x}}_2=\ddot{x}=6\dot{x}-9x+t=6x_2-9x_1+t$$

因此我们的一阶方程组是：

$${\dot{x}}_1=x_2$$

$${\dot{x}}_2=6x_2-9x_1+t$$

如果你稍微练习一下，你就会变得很熟练。分离最高项，定义一个新变量及其导数，然后替换。

状态空间形式的一阶微分方程

我们需要转化的$n$阶微分方程为：

$$a_n\frac{d^{n}x}{d{t}^n}+a_{n-1}\frac{d^{n-1}x}{d{t}^{n-1}}+...+a_2\frac{d^{2}x}{d{t}^2}+a_1\frac{dx}{dt}+a_0=u$$

利用上面的方法，定义新的变量用于替换：

$$\frac{d{x}_1}{dt}=x_2,\frac{d{x}_2}{dt}=x_3,...,\frac{d{x}_{n-1}}{dt}=x_n$$

将最高项分离到一边：

$$\frac{d{x}_n}{dt}=-\frac{1}{a_n}\sum _{i=0}^{n-1}{a}_i{x}_{i+1}+\frac{1}{a_n}u$$

替换之后，使用向量矩阵表示法，我们得到：

$$\left[\begin{array}{c}\frac{d{x}_1}{dt}\\ \frac{d{x}_2}{dt}\\ ⋮\\ \frac{d{x}_n}{dt}\end{array}\right]=\left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\\ ⋮\\ {\dot{x}}_n\end{array}\right]=\left[\begin{array}{ccccc}0& 1& 0& \cdots & 0\\ 0& 0& 1& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ -\frac{a_0}{a_n}& -\frac{a_1}{a_n}& -\frac{a_2}{a_n}& \cdots & -\frac{a_{n-1}}{a_n}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_n\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ ⋮\\ \frac{1}{a_n}\end{array}\right]u$$

然后可以记为$\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{Bu}$。

求非时变系统的状态转移矩阵

从上文的内容，我们可以将描述物理系统模型的高阶微分方程，转化成一阶微分方程组。即用状态空间的形式表示系统方程：

$$\dot{\mathbf{x}}=\mathbf{Ax}$$

式中，$\mathbf{A}$是系统动力学矩阵。我们希望找到将状态$\mathbf{x}$经过如下等式，传播到时间间隔$\Delta t$的状态转移矩阵$\mathbf{F}$：

$$\mathbf{x}(t_k)=\mathbf{F}(\Delta t)\mathbf{x}(t_{k-1})$$

换句话说，$\mathbf{A}$是一组连续的微分方程，我们需要的$\mathbf{F}$是一组离散的线性方程，因而计算$\mathbf{A}$在离散时间步长上的变化。

为了描述方便，删除$t_k$和$(\Delta t)$，简单表示为：

$${\mathbf{x}}_k=\mathbf{F}{\mathbf{x}}_{k-1}$$

广义地说，有三种常见的方法来寻找卡尔曼滤波器的$\mathbf{F}$矩阵。最常用的技术是矩阵指数法、线性非时变理论（也称为LTI系统理论）、数值技术。你可能知道一些其他方法，但这三个是你最有可能在卡尔曼滤波文献和实践遇到的。

矩阵指数法

例如，方程$\frac{dx}{dt}=kx$的解可通过以下公式得出：

$$\frac{dx}{dt}=kx$$

$$\frac{dx}{x}=kdt$$

$$\int \frac{1}{x}dx=\int kdt$$

$$logx=kt+c$$

$$x=e^{kt+c}=e^c{e}^{kt}=c_0{e}^{kt}$$

假设，$t=0$时，$x=x_0$。那么有：

$$x_0=c_0$$

$$x=x_0{e}^{kt}$$

我们利用数学中的相似性，类比出矩阵形式，求解一阶方程：

$$\dot{\mathbf{x}}=\mathbf{Ax}$$

$$\mathbf{x}(0)={\mathbf{x}}_0$$

其中$\mathbf{A}$是常数矩阵，结果是：

$$\mathbf{x}={\mathbf{x}}_0{e}^{\mathbf{A}t}$$

代入$\mathbf{F}=e^{\mathbf{A}t}$，我们可以写：

$${\mathbf{x}}_k=\mathbf{F}{\mathbf{x}}_{k-1}$$

这就是我们需要找的形式！我们把求基本矩阵的问题归结为求$e^{\mathbf{A}t}$的值的问题。$e^{\mathbf{A}t}$被称为矩阵指数，可以用以下幂级数计算：

$$e^{\mathbf{A}t}=\mathbf{I}+\mathbf{A}t+\frac{(\mathbf{A}t{)}^2}{2!}+\frac{(\mathbf{A}t{)}^3}{3!}+...$$

这个级数是通过对$e^{\mathbf{A}t}$进行泰勒级数展开得到的，我在这里不讨论。

我们用这个方法来解牛顿方程的$\dot{\mathbf{x}}=\mathbf{Ax}$。用$v$代替$\dot{x}$，假设速度恒定，得到线性矩阵向量形式：

$$\left[\begin{array}{c}\dot{x}\\ \dot{v}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}x\\ v\end{array}\right]$$

这是一个一阶微分方程，所以我们可以设置$\mathbf{A}=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]$，然后求解下面的方程。我用区间$\Delta t$代替$t$来强调基本矩阵是离散的：

$$\mathbf{F}=e^{\mathbf{A}\Delta t}=\mathbf{I}+\mathbf{A}\Delta t+\frac{(\mathbf{A}\Delta t{)}^2}{2!}+\frac{(\mathbf{A}\Delta t{)}^3}{3!}+...$$

如果你执行乘法，你会发现${\mathbf{A}}^2=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right]$，这意味着$\mathbf{A}$的所有高次幂也都是$0$。因此，我们得到了一个精确的答案，而不需要无穷多的项：

$$\mathbf{F}=e^{\mathbf{A}\Delta t}=\mathbf{I}+\mathbf{A}\Delta t=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]+\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\Delta t=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$$

我们将其插入${\mathbf{x}}_k=\mathbf{F}{\mathbf{x}}_{k-1}$以获得：

$${\mathbf{x}}_k=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\mathbf{x}}_{k-1}$$

这正是我们在多元卡尔曼滤波一章中解析导出的恒速卡尔曼滤波的状态转移矩阵。

SciPy的linalg模块包括一个函数expm()，用于计算矩阵指数。它不使用泰勒级数方法，而是使用帕德近似。计算矩阵指数的方法很多（至少有19种），而且都有数值计算上的困难。你应该意识到这些问题，尤其是当$\mathbf{A}$很大的时候。如果你搜索帕德近似矩阵指数，你会发现许多论文都在致力于研究这个问题。

在实践中，这可能与你无关，因为对于卡尔曼滤波器，我们通常只取泰勒级数的前两项。有趣的是，求解$e^{\mathbf{A}t}$的最受欢迎的方法是使用广义ODE解算器。换句话说，他们做的和我们做的相反——把$\mathbf{A}$转化成一组微分方程，然后用数值技术求解这组方程！

下面是一个使用expm()求解$e^{\mathbf{A}t}$的示例。

import numpy as np
from scipy.linalg import expm

dt = 0.1
A = np.array([[0, 1], 
          [0, 0]])
expm(A*dt)

array([[1. , 0.1],
    [0. , 1. ]])

示例：质量弹簧减振器模型

假设我们想跟踪一个固定在弹簧或减震器上的重物的运动，例如汽车的悬架。以$m$为质量，$k$为弹簧常数，$c$为阻尼力，在一定输入$u$下的运动方程为：

$$m\frac{d^{2}x}{d{t}^2}+c\frac{dx}{dt}+kx=u$$

为了方便记法，我将把它写成:

$$m\ddot{x}+c\dot{x}+kx=u$$

我可以通过设置$x_1(t)=x(t)$将其转化为一阶方程组，然后代入如下：

$$x_1=x$$

$$x_2={\dot{x}}_1$$

$${\dot{x}}_2={\ddot{x}}_1=\ddot{x}$$

通常，为了便于记法，我把$(t)$去掉了。这就得到了方程：

$$m{\dot{x}}_2+c{x}_2+k{x}_1=u$$

解${\dot{x}}_2$得到一阶方程：

$${\dot{x}}_2=-\frac{c}{m}{x}_2-\frac{k}{m}{x}_1+\frac{1}{m}u$$

我们将其转化为矩阵形式：

$$\left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -\frac{k}{m}& -\frac{c}{m}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{m}\end{array}\right]u$$

现在我们用指数矩阵来求状态转移矩阵：

$$\Phi (t)=e^{\mathbf{A}t}=\mathbf{I}+\mathbf{A}t+\frac{(\mathbf{A}t{)}^2}{2!}+\frac{(\mathbf{A}t{)}^3}{3!}+...$$

前两个项计算得到：

$$\mathbf{F}=\left[\begin{array}{cc}1& t\\ -(k/m)t& 1-(c/m)t\end{array}\right]$$

这可能会，也可能不会给你足够的精度。你可以通过计算$\frac{(\mathbf{A}t{)}^2}{2!}$轻松地检查这一点！

线性非时变理论

先介绍一下什么是非时变性。如果系统的行为依赖于时间，我们可以说这个动态系统是由下面的一阶微分方程描述的：

$$g(t)=\dot{x}$$

但是，如果系统是不随时间变化的，则方程的形式为：

$$f(x)=\dot{x}$$

非时变性是什么意思？比如家用立体声音响。如果你在时间$t$向它输入一个信号$x$，它将输出一些信号$f(x)$。如果改为在时间$t+\Delta t$执行输入，则输出信号将是相同的$f(x)$。

反例是$x(t)=sin(t)$，系统$f(x)=tx(t)=tsin(t)$，这不是非时变的。由于$t$的相乘，值在不同时间会导致不同的结果。飞机也不是非时变的，因为飞机随着时间的推移将燃烧燃油，从而减轻重量，而较低的重量会导致不同的行为。

我们可以对每一边分别积分来解非时变系统，然而，积分非时变方程$\dot{x}=f(x)$并不总是那么简单。使用分离变量技术，我们可以分别对每一边进行积分：

$$\frac{dx}{dt}=f(x)$$

$${\int }_{x_0}^x\frac{1}{f(x)}dx={\int }_{t_0}^{t}dt$$

如果我们让$\mathbf{F}(x)={\int }_{}^{}\frac{1}{f(x)}dx$，我们得到：

$$\mathbf{F}(x)-\mathbf{F}(x_0)=t-t_0$$

然后我们用：

$$\mathbf{F}(x)=\mathbf{F}(x_0)+t-t_0$$

$$x={\mathbf{F}}^{-1}[\mathbf{F}(x_0)+t-t_0]$$

换句话说，我们需要找到$\mathbf{F}$的逆（反函数）。然而这不是很简单的事，大量课程都致力于寻找普适性的解决方案。

然而，许多简单形式的$f(x)$要么没有解析解，要么就是解决起来极为困难。因而，工程师转向状态空间方法来寻找近似解。

矩阵指数的优点是，我们可以用它来处理任何一组非时变的微分方程。然而，即使方程不是非时变的，我们也经常使用这种技术。比如当飞机飞行时，它燃烧燃料，重量减轻。但是，短时间内的重量损失可以忽略不计，因此系统在该时间步长内几乎是线性的。只要时间步长足够短，我们的答案还是相当准确的。

线性非时变理论，又被称为LTI系统理论，它给出了一种利用拉普拉斯逆变换求$\Phi$的方法（即状态转移矩阵）。现在你估计已经完全懵了，我不会在本文中使用拉普拉斯逆变换，只做个浅显的介绍。LTI系统理论告诉我们：

$$\Phi (t)={\mathcal{L}}^{-1}[(s\mathbf{I}-\mathbf{A}{)}^{-1}]$$

我不想讨论这个问题，只想说拉普拉斯变换$\mathcal{L}$把一个信号转换到一个不包括时间的空间$s$，但是找到上面方程的解并非易事。如果你感兴趣，维基百科关于LTI系统理论的文章提供了一个介绍。我提到LTI是因为你会发现一些文献使用它来设计较困难问题的卡尔曼滤波矩阵。

数值技术

最后，还有一些数值方法来寻找$\mathbf{F}$。随着滤波器变得越来越大，寻找解析解变得非常乏味（尽管有类似SymPy这样的软件包）。C.F.van Loan已经开发出一种技术，可以在数值上同时找到$\mathbf{\Phi }$和$\mathbf{Q}$。给定连续模型：

$$\dot{x}=Ax+Gw$$

其中，$w$是单位白噪声。C.F.van Loan的方法同时计算${\mathbf{F}}_k$和${\mathbf{Q}}_k$。

在FilterPy中已经实现了C.F.van Loan的方法。你可以按如下方式使用：

from filterpy.common import van_loan_discretization

A = np.array([[0., 1.], [-1., 0.]])
G = np.array([[0.], [2.]]) # white noise scaling
F, Q = van_loan_discretization(A, G, dt=0.1)

过程噪声矩阵的设计

一般来说，$\mathbf{Q}$矩阵的设计是卡尔曼滤波器设计中最困难的方面之一。这是由几个因素造成的。首先，这需要一个良好的信号理论基础。第二，我们正试图对我们所知甚少的事物中的噪声进行建模。比如：试着为抛出的棒球的过程噪音进行建模。我们可以将其建模为一个在空气中运动的球体，但这会留下许多未知的因素——球的旋转和自旋衰减、有缝线的球的阻力系数、风和空气密度的影响等等。对于给定的过程模型，我们建立了精确的数学方程，但是由于过程模型是不完美的，$\mathbf{Q}$的结果也不会是一个可以轻易确定的定值。

这对卡尔曼滤波器有很多影响。如果$\mathbf{Q}$太小，那么滤波器会对其预测模型过于相信，最终导致会偏离观测值；如果$\mathbf{Q}$太大，则滤波器将较容易受到观测噪声的不适当影响，导致结果次优。在实践中，我们花费大量时间运行、模拟和评估数据集，以尝试为$\mathbf{Q}$选择一个合适的值。但是让我们从数学开始。

我们假设一个运动学系统——一个可以用牛顿运动方程建模的系统。我们可以对这个系统做一些不同的假设。

我们一直在使用：

$$\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{Bu}+\mathbf{w}$$

其中，$\mathbf{w}$是过程噪声。运动学系统是连续的——它们的输入和输出可以在任意时间点变化。然而，我们的卡尔曼滤波器是离散的（卡尔曼滤波器有连续的形式，但我们不涉及它们）。我们定期对系统进行采样。因此，我们必须找到上述方程中噪声项的离散表示。这取决于我们对噪音行为的假设。我们将考虑两种不同的噪声模型。

连续白噪声模型

我们用牛顿方程建立运动系统模型。我们要么使用位置和速度，要么使用位置、速度和加速度作为系统的模型。没有什么能阻止我们走得更远——我们可以模拟急动、震动、啪啪声等等，以达到一个更精确的模型。我们通常不会这样做，因为在实际系统的动力学之外添加项会降低估计值精度。

假设我们需要对位置、速度和加速度进行建模，然后我们可以假设每个离散时间步的加速度是恒定的。当然，系统中存在过程噪声，因此加速度实际上不是恒定的。在本文中，我们将假设加速度的变化为，连续时间零均值的白噪声$w(t)$。换句话说，我们假设随着时间的推移，速度微小变化的平均值为0（零均值）。

由于噪声是连续变化的，我们需要积分，以得到我们所选择的离散化区间的离散噪声。我们在这里不作证明，但噪声的离散化方程是：

$$\mathbf{Q}={\int }_0^{\Delta t}\mathbf{F}(t){\mathbf{Q}}_c{\mathbf{F}}^T(t)dt$$

其中，${\mathbf{Q}}_c$是连续的噪音。$\mathbf{F}(t){\mathbf{Q}}_c{\mathbf{F}}^T(t)$是基于我们的过程模型$\mathbf{F}(t)$，在瞬间$t$的连续噪声的投影。我们想知道在离散区间$\Delta t$上向系统添加了多少噪声，因此我们在区间$[0,\Delta t]$上积分。

我们知道牛顿体系的基本矩阵是：

$$\mathbf{F}=\left[\begin{array}{ccc}1& \Delta t& \Delta t^2/2\\ 0& 1& \Delta t\\ 0& 0& 1\end{array}\right]$$

我们将连续噪声定义为：

$${\mathbf{Q}}_c=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right]{\Phi }_s$$

其中，${\Phi }_s$是白噪声的频谱密度。这是可以推导出来的，但超出了本文的研究范围。有关详细信息，请参阅任何关于随机过程的标准书籍。在实践中，我们通常不知道噪声的频谱密度，因此这就变成了一个工程因素——我们通过实验调整这个数字，直到我们的滤波器达到预期效果为止。你可以看到通过状态转移矩阵的相乘，将频谱密度矩阵${\Phi }_s$有效地指定给加速度项。这是有道理的：我们假设，除了噪声引起的变化外，系统具有恒定的加速度。而噪音改变了加速度。

我们可以自己进行这些计算，但我更喜欢用Symphy来解方程。

$${\mathbf{Q}}_c=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right]{\Phi }_s$$

import sympy
from sympy import (init_printing, Matrix, MatMul, 
                   integrate, symbols)

init_printing(use_latex='mathjax')
dt, phi = symbols('\Delta{t} \Phi_s')
F_k = Matrix([[1, dt, dt**2/2],
              [0,  1,      dt],
              [0,  0,       1]])
Q_c = Matrix([[0, 0, 0],
              [0, 0, 0],
              [0, 0, 1]])*phi

Q = integrate(F_k * Q_c * F_k.T, (dt, 0, dt))

# factor phi out of the matrix to make it more readable
Q = Q / phi
MatMul(Q, phi)

2阶的方程为：$\left[\begin{array}{ccc}\frac{\Delta t^5}{20}& \frac{\Delta t^4}{8}& \frac{\Delta t^3}{6}\\ \frac{\Delta t^4}{8}& \frac{\Delta t^3}{3}& \frac{\Delta t^2}{2}\\ \frac{\Delta t^3}{6}& \frac{\Delta t^2}{2}& \Delta t\end{array}\right]{\Phi }_x$

为了完整性，让我们计算0阶和1阶方程：

F_k = Matrix([[1]])
Q_c = Matrix([[phi]])

print('0th order discrete process noise')
integrate(F_k*Q_c*F_k.T,(dt, 0, dt))

0阶的方程为：$[\Delta t{\Phi }_x]$

F_k = Matrix([[1, dt],
              [0, 1]])
Q_c = Matrix([[0, 0],
              [0, 1]]) * phi

Q = integrate(F_k * Q_c * F_k.T, (dt, 0, dt))

print('1st order discrete process noise')
# factor phi out of the matrix to make it more readable
Q = Q / phi
MatMul(Q, phi)

1阶的方程为：$\left[\begin{array}{cc}\frac{\Delta t^3}{3}& \frac{\Delta t^2}{2}\\ \frac{\Delta t^2}{2}& \Delta t\end{array}\right]{\Phi }_x$

分段白噪声模型

另一个噪声模型假设，最高阶项（比如加速度）在每个时间段内是恒定的，但在每个时间段之间是不同的，并且每个时间段之间都不相关。换句话说，每经过一个时间步，加速度都有一个不连续的跳跃。这与上面的模型略有不同，我们假设最后一个项应用了一个连续变化的噪声信号。

我们将把它建模为：

$$f(x)=Fx+\Gamma w$$

其中，$\Gamma$是系统的噪声增益，$w$是恒定的分段加速度（或速度/急动等）。

让我们从一阶系统开始。在这种情况下，我们有状态转移函数：

$$\mathbf{F}=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$$

在一个时间段内，速度变化为$w(t)\Delta t$，位置变化为$w(t)\Delta t^2/2$，这导致：

$$\Gamma =\left[\begin{array}{c}\frac{1}{2}\Delta t^2\\ \Delta t\end{array}\right]$$

然后计算过程噪声的协方差：

$$Q=E[\Gamma w(t)w(t){\Gamma }^T]=\Gamma {\sigma }_v^2{\Gamma }^T$$

我们可以用symphy计算如下：

var = symbols('sigma^2_v')
v = Matrix([[dt**2 / 2], [dt]])

Q = v * var * v.T

# factor variance out of the matrix to make it more readable
Q = Q / var
MatMul(Q, var)

1阶的方程为：$\left[\begin{array}{cc}\frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}\\ \frac{\Delta t^3}{2}& \Delta t^2\end{array}\right]{\sigma }_v^2$

二阶系统采用相同的数学方法：

$$\mathbf{F}=\left[\begin{array}{ccc}1& \Delta t& \Delta t^2/2\\ 0& 1& \Delta t\\ 0& 0& 1\end{array}\right]$$

这里我们假设白噪声是一个离散时间的$\Gamma$过程。我们有：

$$\Gamma =\left[\begin{array}{c}\frac{1}{2}\Delta t^2\\ \Delta t\\ 1\end{array}\right]$$

这个模型没有真实性，它只是方便且提供了良好的结果。例如，我们完全可以假设噪声是一个更复杂的方程。

然后计算过程噪声的协方差：

$$Q=E[\Gamma w(t)w(t){\Gamma }^T]=\Gamma {\sigma }_v^2{\Gamma }^T$$

我们可以用symphy计算如下：

var = symbols('sigma^2_v')
v = Matrix([[dt**2 / 2], [dt], [1]])

Q = v * var * v.T

# factor variance out of the matrix to make it more readable
Q = Q / var
MatMul(Q, var)

2阶的方程为：$\left[\begin{array}{ccc}\frac{\Delta t^4}{4}& \frac{\Delta t^3}{2}& \frac{\Delta t^2}{2}\\ \frac{\Delta t^3}{2}& \Delta t^2& \Delta t\\ \frac{\Delta t^2}{2}& \frac{\Delta t^2}{2}& \Delta t\end{array}\right]{\sigma }_v^2$

我们不能说这个模型比连续模型更正确或更不正确——两者都近似于实际对象所发生的情况。只有经验和实验才能引导你找到合适的模型。在实践中，你通常会发现任何一种模型都能提供合理的结果，但通常某一种模型的性能会优于其他的模型。

第二个模型的优点是，我们可以根据${\sigma }^2$对噪声进行建模，而${\sigma }^2$可以用我们期望的运动和误差量来描述。第一个模型要求我们指定光谱密度，这不是很直观，但它更容易处理变化的时间样本，因为噪声在整个时间段内被积分。但是，这些不是固定的规则——根据测试滤波器的性能和你对物理模型的了解，选择其中更优的模型（或你自己设计的模型）。

一个好的经验法则是将$\sigma$设置在$\frac{1}{2}\Delta a$到$\Delta a$，其中$\Delta a$是采样周期之间加速度变化的最大量。在实践中，我们选择一个数字，对数据进行模拟，然后选择一个效果良好的值。

用FilterPy计算Q

FilterPy提供了几个方法来计算$\mathbf{Q}$矩阵。Q_continuous_white_noise()函数计算指定$\Delta t$和光谱密度下的$\mathbf{Q}$。

from filterpy.common import Q_continuous_white_noise
from filterpy.common import Q_discrete_white_noise

Q = Q_continuous_white_noise(dim=2, dt=1, spectral_density=1)
print(Q)

[[0.333 0.5  ]
 [0.5   1.   ]]

Q = Q_continuous_white_noise(dim=3, dt=1, spectral_density=1)
print(Q)

[[0.05  0.125 0.167]
 [0.125 0.333 0.5  ]
 [0.167 0.5   1.   ]]

Q_discrete_white_noise()函数假设噪声的分段模型，计算$\mathbf{Q}$。

Q = Q_discrete_white_noise(2, var=1.)
print(Q)

[[0.25 0.5 ]
 [0.5  1.  ]]

Q = Q_discrete_white_noise(3, var=1.)
print(Q)

[[0.25 0.5  0.5 ]
 [0.5  1.   1.  ]
 [0.5  1.   1.  ]]

Q的简化

许多处理方法对$\mathbf{Q}$使用更简单的形式：保留其中的某一个噪声项，其余设置为零。这是否合理？好吧，考虑一下短时间$\Delta t$ 下的$\mathbf{Q}$值。

import numpy as np

np.set_printoptions(precision=8)
Q = Q_continuous_white_noise(
    dim=3, dt=0.05, spectral_density=1)
print(Q)
np.set_printoptions(precision=3)

[[0.00000002 0.00000078 0.00002083]
 [0.00000078 0.00004167 0.00125   ]
 [0.00002083 0.00125    0.05      ]]

我们可以看到，大多数项都非常小。回想一下，使用此矩阵的唯一等式是：

$$\mathbf{P}={\mathbf{FPF}}^T+\mathbf{Q}$$

如果$\mathbf{Q}$的值相对于$\mathbf{P}$来说很小，那么它对$\mathbf{P}$的计算结果几乎没有贡献。将$\mathbf{Q}$设置为零矩阵，除了最右下角项：

$$\mathbf{Q}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& {\sigma }^2\end{array}\right]$$

虽然不正确，但通常是一个有用的近似值。但如果你为一个重要的项目这样做，你将不得不进行大量的测试，以确保你的滤波器在各种情况下工作。

其中的某一个噪声项：变化最快的变量。如果状态是$\mathbf{x}=\left[\begin{array}{cccccc}x& \dot{x}& \ddot{x}& y& \dot{y}& \ddot{y}\end{array}\right]$，那么$\mathbf{Q}$将是$6\times 6$。那么，$\mathbf{Q}$中$\ddot{x}$和$\ddot{y}$元素项对应的方差，必须设置为非零。

后验协方差的稳定计算

我已经给出了计算后验协方差的方程：

$$\mathbf{P}=(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}$$

严格意义上说，这是正确的。但在FilterPy中，这不是计算它的方式，在那里使用约瑟夫方程：

$${\mathbf{P}=(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}(\mathbf{I}-\mathbf{KH})}^T+{\mathbf{KRK}}^T$$

这不是一个BUG，使用它有几个原因。首先，减法$\mathbf{I}-\mathbf{KH}$可能由于浮点计算的不精确性导致矩阵结果不对称。而协方差矩阵必须是对称的，因此非对称通常会导致卡尔曼滤波器发散，甚至由于通过不了NumPy中内置的检查代码而引发异常。

保持对称性的传统方法是以下公式：

$${\mathbf{P}=(\mathbf{P}+\mathbf{P}}^T)/2$$

这是安全的，因为协方差矩阵中的所有元素都满足${\sigma }_{ij}={\sigma }_{ji}$。因此，如果两个值因浮点错误而出现偏差，则此操作会平均两个值之间的误差。

如果你看一下约瑟夫形式，你会发现有两个类似${\mathbf{ABA}}^T$形式的项，所以它都保持对称。但是这个等式是从哪里来的，为什么我要用它而不是下式呢？

$$\mathbf{P}=(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}$$

$${\mathbf{P}=(\mathbf{P}+\mathbf{P}}^T)/2$$

让我们推导一下，你需要理解推导过程来理解方程的目的。更重要的是，如果你滤波时因数值不稳定导致发散，你需要学会自己对问题进行诊断。

首先，解释一下符号的含义。$\mathbf{x}$是我们系统的真实状态，$\bar{\mathbf{x}}$是我们系统估计的先验状态，$\hat{\mathbf{x}}$是我们系统估计的后验状态。

鉴于此，我们可以将模型定义为：

$${\mathbf{x}}_{k+1}={\mathbf{F}}_k{\mathbf{x}}_k+{\mathbf{w}}_k$$

$${\mathbf{z}}_k={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{v}}_k$$

换句话说，下一刻的系统状态量${\mathbf{x}}_{k+1}$是当前系统状态量${\mathbf{x}}_k$，经过${\mathbf{F}}_k$状态转移后，再加上一些噪声${\mathbf{w}}_k$。

请注意，这只是定义。没有一个系统完全遵循数学模型，所以我们用噪声项${\mathbf{w}}_k$来建模。由于传感器误差，没有完美的观测，因此我们用噪声项${\mathbf{v}}_k$来建模。

为了书写简单一些，我将在剩下的推导过程中省略下标$k$。

现在我们将估计误差定义为真实状态和估计状态之间的差异：

$$\mathbf{e}=\mathbf{x}-\hat{\mathbf{x}}$$

同样，这只是定义。我们无法利用它来准确计算$\mathbf{e}$，它只是真实状态和估计状态之间的定义差。

这允许我们定义估计误差的协方差，它被定义为${\mathbf{ee}}^T$：

$$\mathbf{P}=E[{\mathbf{ee}}^T]=E[(\mathbf{x}-\hat{\mathbf{x}})(\mathbf{x}-\hat{\mathbf{x}}{)}^T]$$

接下来，我们将后验估计定义为：

$$\hat{\mathbf{x}}=\bar{\mathbf{x}}+\mathbf{K}(\mathbf{z}-\mathbf{H}\bar{\mathbf{x}})$$

这看起来像来自卡尔曼滤波器的方程，这是有充分理由的。但和到目前为止的其他数学公式一样，这只是定义。特别的是，我们没有定义$\mathbf{K}$，你不应该把它看作是卡尔曼增益。因为我们解决的是任何问题，而不仅仅是线性卡尔曼滤波器。这里，$\mathbf{K}$只是0和1之间的某个未指定的混合值。

现在我们有了我们的定义，让我们执行一些替换。

$\mathbf{x}-\hat{\mathbf{x}}$项可以通过替换$\hat{\mathbf{x}}$具有下述定义：

$$(\mathbf{x}-\hat{\mathbf{x}})=\mathbf{x}-(\bar{\mathbf{x}}+\mathbf{K}(\mathbf{z}-\mathbf{H}\bar{\mathbf{x}}))$$

现在我们将$\mathbf{z}$替换为$\mathbf{Hx}+\mathbf{v}$：

$$(\mathbf{x}-\hat{\mathbf{x}})=\mathbf{x}-(\bar{\mathbf{x}}+\mathbf{K}(\mathbf{z}-\mathbf{H}\bar{\mathbf{x}}))$$

$$=\mathbf{x}-(\bar{\mathbf{x}}+\mathbf{K}(\mathbf{Hx}+\mathbf{v}-\mathbf{H}\bar{\mathbf{x}}))$$

$$=(\mathbf{I}-\mathbf{KH})(\mathbf{x}-\bar{\mathbf{x}})-\mathbf{Kv}$$

现在我们可以解$\mathbf{P}$，如果我们注意到$\mathbf{x}-\bar{\mathbf{x}}$的期望值是先验协方差$\bar{\mathbf{P}}$，$\mathbf{v}$的预期值为$E[{\mathbf{vv}}^T]=\mathbf{R}$。

$$\mathbf{P}=E[((\mathbf{I}-\mathbf{KH})(\mathbf{x}-\bar{\mathbf{x}})-\mathbf{Kv})((\mathbf{I}-\mathbf{KH})(\mathbf{x}-\bar{\mathbf{x}})-\mathbf{Kv}{)}^T]$$

$$={(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}(\mathbf{I}-\mathbf{KH})}^T+{\mathbf{KRK}}^T$$

这就是我们证明的过程。

请注意，该方程适用于任何$\mathbf{K}$，而不仅仅是卡尔曼滤波器计算的最佳$\mathbf{K}$。这就是我使用这个等式的原因。在实践中，由滤波器计算出的卡尔曼增益不是最佳值，这是因为现实世界从来都不是真正的线性和高斯，同时在使用浮点计算时也会导致误差。而现实中这个方程，不太可能导致卡尔曼滤波器发散。

$\mathbf{P}=(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}$来自哪里呢？让我们完成推导，这很简单。回想一下，卡尔曼滤波器增益：

$$\mathbf{K}=\bar{\mathbf{P}}{\mathbf{H}}^T(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R}{)}^{-1}$$

现在我们把它代入刚才推导的方程中：

$${(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}(\mathbf{I}-\mathbf{KH})}^T+{\mathbf{KRK}}^T$$

$$=\bar{\mathbf{P}}-\mathbf{KH}\bar{\mathbf{P}}-\bar{\mathbf{P}}{\mathbf{H}}^T{\mathbf{K}}^T+\mathbf{K}(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R}){\mathbf{K}}^T$$

$$=\bar{\mathbf{P}}-\mathbf{KH}\bar{\mathbf{P}}-\bar{\mathbf{P}}{\mathbf{H}}^T{\mathbf{K}}^T+\bar{\mathbf{P}}{\mathbf{H}}^T(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R}{)}^{-1}(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R}){\mathbf{K}}^T$$

$$=\bar{\mathbf{P}}-\mathbf{KH}\bar{\mathbf{P}}$$

$$=(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}$$

因此，当增益为最佳时，$\mathbf{P}=(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}$在数学上是正确的，${(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}(\mathbf{I}-\mathbf{KH})}^T+{\mathbf{KRK}}^T$也是。正如我们已经讨论过的，当增益次优时，后者也是正确的，并且在数值上更稳定。因此，在FilterPy中使用该计算方式。

很有可能你的滤波器仍然存在发散现象，尤其是如果它已经运行了数百乃至数千次循环，你需要检查一下这些等式。有论文提供了这种计算的其他形式，可能更适用于你的问题。如果你在解决真正的工程问题，其中的故障可能意味着设备或生命的损失，你将需要越过这本书，进入工程问题的解决中去。如果你处理的是玩具问题，故障不会造成太大的损害，此时你检测到发散，你可以将$\mathbf{P}$的值重置为某个合理值，然后继续。例如，可以将非对角元素归零，此时矩阵只包含方差，然后可以乘以一个略大于1的常数，以反映刚刚注入滤波器的信息丢失。运用你的想象力，进行测试。

卡尔曼增益方程的推导

在上面，我们把卡尔曼增益带入，证明了后验协方差的正确性。现在我将推导出卡尔曼增益$\mathbf{K}$。

请注意，此处推导不使用贝叶斯方程。我至少见过四种不同的方法来推导卡尔曼滤波方程，现参考Brown的论文。

在上一节中，我们使用了未指定的比例因子$\mathbf{K}$来推导协方差方程的约瑟夫形式。如果我们想要一个最优滤波器，我们需要使用微积分来最小化方程中的误差。你应该很熟悉这个想法，如果你想找到函数$f(x)$的最小值，你可以求导数，并将其设为零：$\frac{df(x)}{dx}=0$。

在我们的问题中，误差由协方差矩阵$\mathbf{P}$表示。特别的，对角线表示状态向量中每个元素的误差（方差）。所以，为了找到最佳增益，我们需要对对角线的迹进行求导。

Brown让我们回忆两个涉及迹的导数的公式：

$$\frac{d(trace(\mathbf{AB}))}{d\mathbf{A}}={\mathbf{B}}^T$$

$$\frac{d(trace({\mathbf{ACA}}^T))}{d\mathbf{A}}=2\mathbf{AC}$$

其中，$\mathbf{AB}$是方阵，$\mathbf{C}$是对称阵。

我们将约瑟夫方程展开为：

$${\mathbf{P}=(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}(\mathbf{I}-\mathbf{KH})}^T+{\mathbf{KRK}}^T$$

$$=\bar{\mathbf{P}}-\mathbf{KH}\bar{\mathbf{P}}-\bar{\mathbf{P}}{\mathbf{H}}^T{\mathbf{K}}^T+\mathbf{K}(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R}){\mathbf{K}}^T$$

现在我们需要求$\mathbf{P}$的迹对$\mathbf{K}$的导数：$\frac{d(trace(\mathbf{P}))}{d\mathbf{K}}$：

• 迹的导数第一项是0，因为表达式中没有$\mathbf{K}$。
• 迹的导数第二项是$(\mathbf{H}\bar{\mathbf{P}}{)}^T$。
• 由于$\bar{\mathbf{P}}{\mathbf{H}}^T{\mathbf{K}}^T$是$\mathbf{KH}\bar{\mathbf{P}}$的转置，而矩阵的迹等于它的转置迹。因此，迹的导数第三项与第二项相同。
• 迹的导数第四项是$2\mathbf{K}(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R})$。

可以得到：

$$\frac{d(trace(\mathbf{P}))}{d\mathbf{K}}=-2(\mathbf{H}\bar{\mathbf{P}}{)}^T+2\mathbf{K}(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R})$$

我们将其设置为零，然后求解$\mathbf{K}$，使误差最小化：

$$-2(\mathbf{H}\bar{\mathbf{P}}{)}^T+2\mathbf{K}(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R})=0$$

$$\mathbf{K}(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R})=(\mathbf{H}\bar{\mathbf{P}}{)}^T$$

$$\mathbf{K}(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R})=\bar{\mathbf{P}}{\mathbf{H}}^T$$

$$\mathbf{K}=\bar{\mathbf{P}}{\mathbf{H}}^T(\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R}{)}^{-1}$$

这个推导并不是特别严谨的，因为我遗漏了一个关键问题：关于为什么最小化迹可以最小化总误差。但我认为到这里就足够了，如果你需要了解更多，可以去阅读更专业的论文。

微分方程的数值积分

我们已经接触了几种数值技术来解决线性微分方程。

但有些方法适用于线性常微分方程，但不适用于非线性方程。例如，试着预测一辆快速转弯的汽车的位置。该汽车通过转动前轮来实现转弯，这使得它们在后轮向前移动时又绕着后轮转动。因此，路径将不断变化，线性预测必然会产生错误的值。

由于这些原因，我们需要知道如何对非线性的常微分方程进行数值积分。这可能是一个需要几本书的庞大主题。然而，我将介绍一些简单的技术，它们将适用于你遇到的大多数问题。

欧拉方法

假设我们需要解决的问题为：

$$y^{{}^{\prime }}=y$$

$$y(0)=1$$

我们碰巧知道确切的答案是$y=e^t$，因为我们之前已经解决了它。但对于任意的常微分方程，我们可能并不知道精确的解。通常我们只知道方程的导数，它等于斜率。我们也知道初始值：$t=0$，$y=1$。如果我们知道这两条信息，我们可以使用$t=0$的斜率和$y(0)$的值来预测$y(1)$的值。我在下面画了这个。

import matplotlib.pyplot as plt

t = np.linspace(-1, 1, 10)
plt.plot(t, np.exp(t))
t = np.linspace(-1, 1, 2)
plt.plot(t,t+1, ls='--', c='k')

你可以看到，在$t=0.1$时，斜率非常接近曲线，但在$t=1$时，斜率却远远偏离曲线。但是，让我们继续使用1作为步长。我们可以看到，在$t=1$时，$y$的估计值为2。现在我们可以计算$t=2$时的值，方法是取$t=1$时曲线的斜率，并将其添加到初始估计中。坡度由$y^{{}^{\prime }}=y$计算，所以斜率是2。

import kf_book.book_plots as book_plots

t = np.linspace(-1, 2, 20)
plt.plot(t, np.exp(t))
t = np.linspace(0, 1, 2)
plt.plot([1, 2, 4], ls='--', c='k')
book_plots.set_labels(x='x', y='y')

在这里，我们看到$y$的下一个估计值是4。错误很快就变得越来越大，而你可能对此无动于衷。但是1是一个非常大的步长。让我们将这个算法放入代码中，并通过使用较小的步长来验证它是否有效。

def euler(t, tmax, y, dx, step=1.):
    ys = []
    while t < tmax:
        y = y + step*dx(t, y)
        ys.append(y)
        t += step
    return ys

def dx(t, y):
    return y

print(euler(0, 1, 1, dx, step=1.)[-1])
print(euler(0, 2, 1, dx, step=1.)[-1])

2.0
4.0

这看起来是正确的。现在让我们绘制一个小得多的步长的结果。

ys = euler(0, 4, 1, dx, step=0.00001)
plt.subplot(1,2,1)
plt.title('Computed')
plt.plot(np.linspace(0, 4, len(ys)),ys)
plt.subplot(1,2,2)
t = np.linspace(0, 4, 20)
plt.title('Exact')
plt.plot(t, np.exp(t))

print('exact answer=', np.exp(4))
print('euler answer=', ys[-1])
print('difference =', np.exp(4) - ys[-1])
print('iterations =', len(ys))

exact answer= 54.598150033144236
euler answer= 54.59705808834125
difference = 0.0010919448029866885
iterations = 400000

这里我们看到误差相当小，但需要大量迭代才能获得三位数的精度。实际上，欧拉的方法对于大多数问题来说太慢了，我们可以使用更好的方法。

在我们继续之前，让我们正式推导欧拉方法，因为它是之后使用的更高级龙格-库塔方法的基础。事实上，欧拉方法是龙格-库塔法的最简单形式。

这里是$y$的泰勒展开式的前三项，无限展开式会给出精确的答案，所以$O(h^4)$表示由于有限次数而产生的误差。

$$y(t_0+h)=y(t_0)+h{y}_{t_0}^{{}^{\prime }}+\frac{1}{2!}{h}^2{y}_{t_0}^{{}^{\prime \prime }}+\frac{1}{3!}{h}^3{y}_{t_0}^{{}^{\prime \prime \prime }}+O(h^4)$$

这里我们可以看到，欧拉方法使用泰勒展开式的前两项。随后的每一项都比前一项小，因此我们确信估算值与正确值不会相差太远。

龙格-库塔方法

龙格-库塔法是数值积分的主力军。在实践中，使用我在这里介绍的龙格-库塔算法将解决你将面临的大多数问题。它在速度、精度和稳定性方面提供了一个很好的平衡，它是一种值得尝试的数值积分方法，除非你有很好的理由选择不同的方法。

我们从微分方程开始：

$$\ddot{y}=\frac{d}{dt}\dot{y}$$

我们可以用函数$f$代替$y$的导数，就像这样：

$$\ddot{y}=\frac{d}{dt}f(y,t)$$

推导这些方程超出了本文的范围，但龙格-库塔的RK4方法是用这些方程定义的。

$$y(t+\Delta t)=y(t)+\frac{1}{6}(k_1+2k_2+2k_3+k_4)+O(\Delta t^4)$$

$$k_1=f(y,t)\Delta t$$

$$k_2=f(y+\frac{1}{2}{k}_1,t+\frac{1}{2}\Delta t)\Delta t$$

$$k_3=f(y+\frac{1}{2}{k}_2,t+\frac{1}{2}\Delta t)\Delta t$$

$$k_4=f(y+k_3,t+\Delta t)\Delta t$$

以下是相应的代码：

def runge_kutta4(y, x, dx, f):
    """computes 4th order Runge-Kutta for dy/dx.
    y is the initial value for y
    x is the initial value for x
    dx is the difference in x (e.g. the time step)
    f is a callable function (y, x) that you supply 
    to compute dy/dx for the specified values.
    """
    k1 = dx * f(y, x)
    k2 = dx * f(y + 0.5*k1, x + 0.5*dx)
    k3 = dx * f(y + 0.5*k2, x + 0.5*dx)
    k4 = dx * f(y + k3, x + dx)
    return y + (k1 + 2*k2 + 2*k3 + k4) / 6.

让我们用这个简单的例子：

$$\dot{y}=t\sqrt{y(t)}$$

其中，

$$t_0=0$$

$$y_0=y_{t_0}=1$$

import math
import numpy as np
t = 0.
y = 1.
dt = .1

ys, ts = [], []

def func(y,t):
    return t*math.sqrt(y)

while t <= 10:
    y = runge_kutta4(y, t, dt, func)
    t += dt
    ys.append(y)
    ts.append(t)

exact = [(t**2 + 4)**2 / 16. for t in ts]
plt.plot(ts, ys)
plt.plot(ts, exact)

error = np.array(exact) - np.array(ys)
print(f"max error {max(error):.5f}")

max error 0.00005

贝叶斯滤波

从离散贝叶斯一章开始，我们使用贝叶斯公式进行滤波。假设我们正在跟踪一个物体。我们把它在特定时间的状态定义为：位置、速度。例如，我们可以将时间$t$的状态写为${\mathbf{x}}_t={\left[\begin{array}{cc}{x}_t& {\dot{x}}_t\end{array}\right]}^T$。

当我们对物体进行观测时，我们可以得到物体的状态或部分状态。传感器有噪声，因此观测结果会被噪声破坏。不过，很明显的是，观测结果是由状态决定的。也就是说，状态的变化可能会改变观测值，但观测值的变化不会改变状态。

在滤波中，我们的目标是计算从时间0到时间$t$的状态${\mathbf{x}}_{0:t}$的最佳估计。如果我们已经知道${\mathbf{x}}_{0:t}$，那么计算对应于这些状态的观测值${\mathbf{z}}_{0:t}$就很简单。然而，如果我们已经知道一组观测结果${\mathbf{z}}_{0:t}$，想要计算相应的状态${\mathbf{x}}_{0:t}$，这被称为统计反演，因为我们试图从输出中来计算输入。

反演是一个困难的问题，因为通常没有唯一的解决方案。对于给定的一组状态${\mathbf{x}}_{0:t}$，只有一组可能的观测值（加上噪声）；但对于给定的一组观测值，有许多不同的状态可能导致。

回想一下贝叶斯定理：

$$P(x|z)=\frac{P(z|x)P(x)}{P(z)}$$

其中，$P(z|x)$是观测的似然概率，$P(x)$是基于我们的过程模型的先验概率，$P(z)$是归一化常数。$P(x|z)$是后验概率，或者是包含观测值$z$后的分布。

统计反演就是从$P(z|x)$到$P(x|z)$，我们的滤波问题的解决方案可以表示为：

$$P({\mathbf{x}}_{0:t}|{\mathbf{z}}_{0:t})=\frac{P({\mathbf{z}}_{0:t}|{\mathbf{x}}_{0:t})P({\mathbf{x}}_{0:t})}{P({\mathbf{z}}_{0:t})}$$

在下一次观测${\mathbf{z}}_{t+1}$之前，一切都很好。此时，我们需要重新计算$0:t+1$范围内的整个表达式。

实际上，这很难解决，因为我们正试图计算，整个时间步长范围内后验分布$P({\mathbf{x}}_{0:t}|{\mathbf{z}}_{0:t})$的状态。但是，如果刚刚收到第十个观测值，我们还会关心第三步的概率分布吗？通常不会。所以我们放宽了要求，只计算当前步的分布。即：

$${\mathbf{x}}_k\sim P({\mathbf{x}}_k|{\mathbf{x}}_{k-1})$$

在实践中，这是非常合理的，因为许多事物都具有马尔可夫性质。例如：如果你在停车场开车，下一秒你的位置是否取决于路途中经过的是高速公路，还是城乡公路？不。你在下一秒的位置完全取决于你当前的位置、速度和控制输入，而不是一分钟前发生的事情。因此，汽车具有马尔可夫特性，我们可以在不损失精度或通用性的情况下进行简化。

我们做的下一个简化是根据当前状态${\mathbf{x}}_k$定义观测模型。在给定当前状态下，观测的条件概率为：

$${\mathbf{z}}_k\sim P({\mathbf{z}}_k|{\mathbf{x}}_k)$$

除此之外，我们还需要一个初始条件。因此，我们说初始分布是状态${\mathbf{x}}_0$的概率:

$${\mathbf{x}}_0\sim P({\mathbf{x}}_0)$$

这些项都被包含在贝叶斯方程中。如果我们有状态${\mathbf{x}}_0$，我们可以估计的第一个观测值$P({\mathbf{x}}_1|{\mathbf{z}}_1)$。运动模型创建了先验值$P({\mathbf{x}}_2|{\mathbf{x}}_1)$。我们将其放入到贝叶斯定理中进行计算$P({\mathbf{x}}_2|{\mathbf{z}}_2)$。我们继续使用这种预测-校正算法，递归地计算时间$t$的状态。

计算的数学细节因问题而异。离散贝叶斯和单变量卡尔曼滤波器给出了两种不同的公式，你应该能够都能进行推导。单变量卡尔曼滤波器的状态量是标量，且假设噪声和过程都是线性模型，受零均值、不相关高斯噪声的影响。

多元卡尔曼滤波器做出了相同的假设，但状态量和观测量是向量，而不是标量。卡尔曼博士能够证明，如果这些假设都成立，那么卡尔曼滤波器在最小二乘意义上是最优的。通俗地说，这意味着无法从噪声观测中获得更多信息。

在我继续之前，我想再谈谈统计反演。正如Calvetti和Somersalo在《贝叶斯科学计算导论》中所写：我们采用了贝叶斯的观点，随机性只是意味着缺乏信息。我们的状态量参数化了原则上可以被观测或计算的物理量：速度、空气阻力等。但我们缺乏足够的信息来计算或观测它们的值，所以我们选择它们作为随机变量。严格来说，它们不是随机的，因此这是一种主观的立场。

他们用了整整一章来讨论这个话题，我摘抄其中一段。贝叶斯滤波器是有效的，因为我们将统计特性归因于未知参数。在卡尔曼滤波器的情况下，我们有封闭形式的解决方案来寻找最佳估计。而其他滤波器，如离散贝叶斯滤波器或粒子滤波器，我们将在后面的章节中介绍，以一种更特别、非最优的方式对概率进行建模。我们将缺乏的信息视为随机变量，将该随机变量描述为概率分布，然后使用贝叶斯定理来解决统计推断问题。

相关阅读

• Kalman-and-Bayesian-Filters-in-Python/07-Kalman-Filter-Math