用一个简单模型推导卡尔曼滤波理论

卡尔曼滤波理论由鲁道夫·卡尔曼于1960年提出，随后在解决“阿波罗计划”中航天器的导航问题时获得成功。

卡尔曼滤波理论可以高效地处理测量误差。广泛的测量需求和测量误差的客观存在使它备受关注，从控制科学到电子信息，从航空航天到人工智能，很多领域都有它的身影。

然而仅凭“应用广泛”还不足以说明它的价值，事实上，它常出现在众多领域的高阶部分。有评论说，它是20世纪重要的数学发现之一。

我们用一个简单模型，逐步推导经典卡尔曼滤波理论，体会它的思想方法。

渐入佳境

今有秦岭冷杉，\(n\) 次测量的树高为 \(x_1，x_2，x_3，\ldots，x_n\)，通常的做法是取平均数作为测量结果。

\(\overline{x} = \frac{1}{n}(x_1 + x_2 + x_3 + \ldots + x_n) = \frac{1}{n}\sum\limits_{i=1}^{n}{x_i}\)

下面通过方程变形，把 \(x_n\) 分离出来：

\(\overline{x} = \frac{1}{n}\sum\limits_{i=1}^{n-1}{x_i} + \frac{1}{n}x_n = \frac{n-1}{n}\frac{1}{n-1}\sum\limits_{i=1}^{n-1}{x_i} + \frac{1}{n}x_n = \frac{n-1}{n}\hat{x_{n-1}} + \frac{1}{n}x_n\)

即 \(\overline{x} = (1 - \frac{1}{n})\hat{x_{n-1}} + \frac{1}{n}x_n\)，

其中 \(\hat{x_{n-1}} = \frac{1}{n-1}\sum\limits_{i=1}^{n-1}{x_i}\)，是通过前 \((n-1)\) 次测量对树高的估计。

继续把 \(x_{n-1}\) 分离出来：

\(\hat{x_{n-1}} = \frac{1}{n-1}\sum\limits_{i=1}^{n-2}{x_i} + \frac{1}{n-1}x_{n-1} = \frac{n-2}{n-1}\frac{1}{n-2}\sum\limits_{i=1}^{n-2}{x_i} + \frac{1}{n-1}x_{n-1} = \frac{n-2}{n-1}\hat{x_{n-2}} + \frac{1}{n-1}x_{n-1}\)

即 \(\hat{x_{n-1}} = (1 - \frac{1}{n-1})\hat{x_{n-2}} + \frac{1}{n-1}x_{n-1}\)，

其中 \(\hat{x_{n-2}} = \frac{1}{n-2}\sum\limits_{i=1}^{n-2}{x_i}\)，是通过前 \((n-2)\) 次测量对树高的估计。

以此类推，有通式：

\(\hat{x_{i}} = (1 - k)\hat{x_{i-1}} + kx_{i}\) ，\((0 < k < 1)\) 方程甲

即第 \(i\) 次的估计 \(\hat{x_{i}}\)，可由第 \((i-1)\) 次的估计 \(\hat{x_{i-1}}\) 融合第 \(i\) 次的测量 \(x_{i}\) 后得出。

这就形成了一种不断融合新数据、进行迭代优化的计算方法。

参数 \(k\) 可以用来调节“上次估计”与“本次测量”的采用比例。

接下来我们寻求一个最优的 \(k\) 。

画龙点睛

定义两个随机变量：

令随机变量 \(X\) 为“上次估计”的误差，即 \(\hat{x_{i-1}}\) 的误差（\(\hat{x_{i-1}} = 真实值 + X\)）。

令随机变量 \(Y\) 为“本次测量”的误差，即 \(x_{i}\) 的误差（\(x_{i} = 真实值 + Y\)）。

假设 \(X\) 与 \(Y\) 相互独立。这意味着“上次估计(之前测量)的误差”与“本次测量的误差”不相关。

假设 \(X\) 与 \(Y\) 均服从高斯分布。这意味着其线性组合 \((1-k)X + kY\)也服从高斯分布。

这两条假设是对数学模型的理想化，也是整个推导过程中的关键。

一方面我们可以据此得到一个简洁的方程，另一方面从现实世界的情况来看，这样的假设往往是合理的。

根据这两条假设，计算随机变量的方差：

\(\mathrm{Var}[(1-k)X + kY] = (1-k)^2\mathrm{Var}(X) + k^2\mathrm{Var}(Y)\)

简记为：

\(p_i = f(k) = (1-k)^2p_{i-1} + k^2q\)

其中 \(p_i\) 为“本次估计”的误差方差，\(p_{i-1}\) 为“上次估计”的误差方差，\(q\) 为“本次测量”的误差方差。

这是一个开口向上的一元二次函数，在顶点（导数为零）处取得最小值。

为使“本次估计”的误差方差最小（估计最可靠），令：

\(f'(k) = 2(p_{i-1}+q)k - 2p_{i-1} = 0\)

解得：

\(k = \frac{p_{i-1}}{p_{i-1} + q}\) 方程乙

余霞成绮

此时，

\(1 - k = \frac{q}{p_{i-1} + q}\)

\(p_i = f(k) = (\frac{q}{p_{i-1} + q})^2p_{i-1} + (\frac{p_{i-1}}{p_{i-1} + q})^2q\)

\(p_i = f(k) = (\frac{p_{i-1}q}{p_{i-1} + q})(\frac{q}{p_{i-1} + q} + \frac{p_{i-1}}{p_{i-1} + q})\)

\(p_i = f(k) = \frac{p_{i-1}q}{p_{i-1} + q} = \frac{q}{p_{i-1} + q} p_{i-1}\)

即：

\(p_i = (1 - k){p_{i-1}}\) 方程丙

至此，我们的推导结束了。

实践已经证明，这个基于假设得出的理论表现不俗。

文末附一段 java 程序，看一下“卡尔曼滤波器”的高效与简洁。

/**
 * 一个简单的卡尔曼滤波器
 */
public class Filter {
    private double x; // 估计值
    private double p; // 估计误差方差
    private final double q; // 测量误差方差

    // 初始化
    public Filter(double x, double p, double q) {
        this.x = x;
        this.p = p;
        this.q = q;
    }

    // 优化估计
    public void calc(double newX) {
        double k = p / (p + q); // 对应 方程乙
        x = (1 - k) * x + k * newX; // 对应 方程甲
        p = (1 - k) * p; // 对应 方程丙
    }

    // 输出结果
    @Override
    public String toString() {
        return "x=" + x + ", p=" + p + ", q=" + q;
    }
}

/**
 * 程序入口
 */
public static void main(String[] args) {
    Scanner scanner = new Scanner(System.in);
    System.out.println("请输入估计值：");
    double x = scanner.nextDouble();
    System.out.println("请输入估计误差方差：");
    double p = scanner.nextDouble();
    System.out.println("请输入测量误差方差：");
    double q = scanner.nextDouble();
    // 初始化滤波器
    Filter filter = new Filter(x, p, q);
    System.out.println("请依次输入测量值(小于零时退出)：");
    // 迭代计算
    while (true) {
        double newX = scanner.nextDouble();
        if (newX < 0) break;
        // 优化估计
        filter.calc(newX);
        // 实时输出
        System.out.println(filter);
    }
    scanner.close();
}