用一个简单模型推导卡尔曼滤波理论
卡尔曼滤波理论由鲁道夫·卡尔曼于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; // 对应 方程丙}// 输出结果@Overridepublic 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();
}