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

卡尔曼滤波理论由鲁道夫·卡尔曼于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}sumlimits_{i=1}^{n}{x_i})

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

(overline{x} = frac{1}{n}sumlimits_{i=1}^{n-1}{x_i} + frac{1}{n}x_n = frac{n-1}{n}frac{1}{n-1}sumlimits_{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}sumlimits_{i=1}^{n-1}{x_i})，是通过前 ((n-1)) 次测量对树高的估计。

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

(hat{x_{n-1}} = frac{1}{n-1}sumlimits_{i=1}^{n-2}{x_i} + frac{1}{n-1}x_{n-1} = frac{n-2}{n-1}frac{1}{n-2}sumlimits_{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}sumlimits_{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)^2mathrm{Var}(X) + k^2mathrm{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(); }