卡尔曼滤波器(2) -- α−β−γ滤波器(例1)

This blog is translated from https://www.kalmanfilter.net/default.aspx.
It’s an excellent tutorial about Kalman Filter written by Alex Becker.

目录
卡尔曼滤波器(1) – 背景知识
卡尔曼滤波器(2) – α−β−γ滤波器(例1)
卡尔曼滤波器(3) – α−β−γ滤波器(例2)
卡尔曼滤波器(4) – α−β−γ滤波器(例3&例4&总结)
卡尔曼滤波器(5) – 一维卡尔曼滤波器(介绍)
卡尔曼滤波器(6) – 一维卡尔曼滤波器(例5&完整模型)
卡尔曼滤波器(7) – 一维卡尔曼滤波器(例6)
卡尔曼滤波器(8) – 一维卡尔曼滤波器(例7&例8)
卡尔曼滤波器(9) – 多维卡尔曼滤波器(前言&预备)
卡尔曼滤波器(10) – 多维卡尔曼滤波器(状态外推方程)
卡尔曼滤波器(11) – 多维卡尔曼滤波器(线性动态系统模型)
卡尔曼滤波器(12) – 多维卡尔曼滤波器(协方差外推方程)
卡尔曼滤波器(13) – 多维卡尔曼滤波器(测量方程)
卡尔曼滤波器(14) – 多维卡尔曼滤波器(简短总结)
卡尔曼滤波器(15) – 多维卡尔曼滤波器(状态更新方程)
卡尔曼滤波器(16) – 多维卡尔曼滤波器(协方差更新方程)
卡尔曼滤波器(17) – 多维卡尔曼滤波器(卡尔曼增益)
卡尔曼滤波器(18) – 多维卡尔曼滤波器(简化的协方差更新方程)
卡尔曼滤波器(19) – 多维卡尔曼滤波器(总结)
卡尔曼滤波器(20) – 多维卡尔曼滤波器(例9)

例1 - 金条重量

在这个例子中，我们来估计静态系统的状态。静态系统是在一段合理的时间内不会更改状态的系统。例如，静态系统可以是一座塔，系统状态是塔的高度。

下面来估计金条的重量。我们的秤没有偏差，即秤的测量没有系统误差，但是测量包括随机噪声。

这里系统是金条，系统状态是金条的重量。假设金条的重量在短时间内不发生变化，即系统的动态模型是恒定的。

为了估计系统的状态（金条的重量），可以进行多次测量并求平均值。

在时间点 NNN，估计值 x^N,N\hat{x}_{N,N}x^N,N 是所有测量值的平均值：
x^N,N=1N(z1+z2+...+zN−1+zN)=1N∑n=1N(zn)(1)\hat{x}_{N,N} = \frac{1}{N}(z_1+z_2+...+z_{N-1}+z_N) = \frac{1}{N}\mathop{\sum_{n=1}^N}(z_n) \tag{1} x^N,N=N1(z1+z2+...+zN−1+zN)=N1n=1∑N(zn)(1)示例符号：


xxx	是重量的真实值
znz_nzn	是第 nnn 次称重的测量值
x^n,n\hat{x}_{n,n}x^n,n	是第 nnn 次 xxx 的估计值（测量出 znz_nzn 之后进行估算）
x^n,n−1\hat{x}_{n,n-1}x^n,n−1	是 xxx 的先验估计值，在时间 n−1n-1n−1 时计算出（测量出 zn−1z_{n-1}zn−1 之后进行估算）
x^n+1,n\hat{x}_{n+1,n}x^n+1,n	是对未来 n+1n+1n+1 时 xxx 的估计，在第 nnn 次测量出 znz_nzn 后进行估算。换言之 x^n+1,n\hat{x}_{n+1,n}x^n+1,n 是一个预测值

注：在文章中，变量头顶带有“尖号”表示它是估计值

本例中的动态模型是恒定的（这句话翻译过来是这样的，意思其实就是指静态模型），因此 x^n+1,n=x^n,n\hat{x}_{n+1,n}=\hat{x}_{n,n}x^n+1,n=x^n,n

若想要按照公式 (1) 估计 x^N,N\hat{x}_{N,N}x^N,N，我们需要记录所有历史测量数据。
然而我们没有笔和纸，电脑也没有足够的内存记录所有的历史数据。
通过下面方法可以通过迭代来算得估计值，每次只需要用到上一次的估计值：

推导过程	备注
x^N,N=1N∑n=1N(zn)\hat{x}_{N,N} = \frac{1}{N}\mathop{\sum_{n=1}^N}(z_n)x^N,N=N1∑n=1N(zn)	平均公式：NNN 次测量值相加，除以 NNN
=1N(∑n=1N−1(zn)+zN)\qquad = \frac{1}{N} \left( \mathop{\sum_{n=1}^{N-1}}(z_n) + z_N\right)=N1(∑n=1N−1(zn)+zN)	前 N−1N-1N−1 次之和，加上第 NNN 次，再除以 NNN
=1N∑n=1N−1(zn)+1NzN\qquad = \frac{1}{N} \mathop{\sum_{n=1}^{N-1}}(z_n) + \frac{1}{N}z_N=N1∑n=1N−1(zn)+N1zN	展开
=1NN−1N−1∑n=1N−1(zn)+1NzN\qquad = \frac{1}{N} \frac{N-1}{N-1} \mathop{\sum_{n=1}^{N-1}}(z_n) + \frac{1}{N}z_N=N1N−1N−1∑n=1N−1(zn)+N1zN	乘以并除以 N−1N-1N−1
=N−1N1N−1∑n=1N−1(zn)+1NzN\qquad = \frac{N-1}{N} \color{Orange}{\frac{1}{N-1} \mathop{\sum_{n=1}^{N-1}} (z_n)} \color{Black} + {\frac{1}{N}z_N}=NN−1N−11∑n=1N−1(zn)+N1zN	橙色的就是上一次的估计值
=N−1Nx^N,N−1+1NzN\qquad = \frac{N-1}{N} \color{Orange}{ \hat{x}_{N,N-1}} \color{Black} + {\frac{1}{N}z_N}=NN−1x^N,N−1+N1zN	橙色的就是上一次的估计值
=x^N,N−1−1Nx^N,N−1+1NzN\qquad =\hat{x}_{N,N-1} - \frac{1}{N} \hat{x}_{N,N-1} + \frac{1}{N}z_N=x^N,N−1−N1x^N,N−1+N1zN	展开
=x^N,N−1+1N(zN−x^N,N−1)\qquad =\hat{x}_{N,N-1} + \frac{1}{N} \left( z_N - \hat{x}_{N,N-1} \right)=x^N,N−1+N1(zN−x^N,N−1)	合并

即：
x^N,N=x^N,N−1+1N(zN−x^N,N−1)(2)\hat{x}_{N,N} = \hat{x}_{N,N-1} + \frac{1}{N} \left( z_N - \hat{x}_{N,N-1} \right) \tag{2} x^N,N=x^N,N−1+N1(zN−x^N,N−1)(2)
x^N,N−1\hat{x}_{N,N-1}x^N,N−1 是 xxx 在 NNN 时刻，基于 N−1N-1N−1 时刻（上一次）的测量值做出的预测状态。
换言之，x^N,N−1\hat{x}_{N,N-1}x^N,N−1 是先验估计值（上一次的估计值）。

正常来说，我们利用公式 (2) 算出了 x^N,N\hat{x}_{N,N}x^N,N，要再用某个方程算出 x^N+1,N\hat{x}_{N+1,N}x^N+1,N 作为下一轮计算的先验估计值，才能继续套用公式 (2) 迭代下去。
这里为了方便教学先讲静态系统，本例中 x^N+1,N=x^N,N\hat{x}_{N+1,N} = \hat{x}_{N,N}x^N+1,N=x^N,N，后面的例子再讲如何从 x^N,N\hat{x}_{N,N}x^N,N 到 x^N+1,N\hat{x}_{N+1,N}x^N+1,N (用状态外推方程)。

公式 (2) 是卡尔曼滤波方程的五个方程之一，叫做状态更新方程(State Update Equation)，其形式和含义为：

公式 (2) 中的系数 1N\dfrac{1}{N}N1 是本例的一个特定值。

在卡尔曼滤波中，这个系数叫做卡尔曼增益(Kalman Gain)，记作 KnK_nKn，下标 nnn 表示它随每次迭代而变化。

KnK_nKn 的发现是Rudolf Kalman的主要贡献之一。

在我们深入学习卡尔曼滤波器前，会使用希腊字母 αn\alpha_nαn 来代替 KnK_nKn。

此时状态更新方程为：
x^n,n=x^n,n−1+αn(zn−x^n,n−1)(3)\hat{x}_{n,n} = \hat{x}_{n,n-1} + \alpha_n \left( z_n - \hat{x}_{n,n-1} \right) \tag{3} x^n,n=x^n,n−1+αn(zn−x^n,n−1)(3)
(zn−x^n,n−1)\left( z_n - \hat{x}_{n,n-1} \right)(zn−x^n,n−1) 为测量残差，也被称为更新(innovation)，它包含了新的信息。

在本例中，1N\dfrac{1}{N}N1 随着 NNN 的增加而减小，这意味着刚开始的时候，我们没有足够的重量信息，因此估算值主要取决于测量值。继续进行下去，1N\dfrac{1}{N}N1 逐渐变小，每次测量的结果在估计过程中占比也越来越少。迭代次数足够多时，新的测量值对估计值的影响可以忽略不计。

继续本例，在做第一次测量之前，可以通过看金条上刻的数字（或粗略估计）得到金条的重量，着称为初始猜测(Initial Guess)，它将是我们的第一个估计。

后面会讲到，卡尔曼滤波器需要预设一个初始猜测值，这个值不用很精准。

估计算法

下图描述了本例中使用的估计算法：

实例

第零次迭代

初始化

我们对金条重量的初始估计是1000克。滤波器初始化操作仅需一次，不会用在下一次迭代中。
x^0,0=1000g\hat{x}_{0,0}=1000g x^0,0=1000g

预测

金条的重量是不变的，因此系统的动态模型是静态的，状态的下一个估计值（预测值）等于初始值：
x^1,0=x^0,0=1000g\hat{x}_{1,0}=\hat{x}_{0,0}=1000g x^1,0=x^0,0=1000g

第一次迭代

步骤1

测量。第一次称重：
z1=1030gz_1=1030g z1=1030g

步骤2

计算增益，在本例中 αn=1n\alpha_n = \dfrac{1}{n}αn=n1，因此：
α1=11=1\alpha_1 = \frac{1}{1}=1 α1=11=1用状态更新方程计算当前估计值：
x^1,1=x^1,0+α1(z1−x^1,0)=1000+1(1030−1000)=1030g\begin{aligned} \hat{x}_{1,1} &= \hat{x}_{1,0} + \alpha_1 (z_1-\hat{x}_{1,0}) \\ &= 1000+1(1030−1000) \\ &= 1030g \end{aligned}x^1,1=x^1,0+α1(z1−x^1,0)=1000+1(1030−1000)=1030g

注：在这个特定的例子中，最初的猜测可以是任何值，因为 α1=1\alpha_1=1α1=1 ，展开式子后初始猜测值(x^1,0\hat{x}_{1,0}x^1,0)在第一次迭代就被消去了。

步骤3

系统的动态模型是静态的，因此金条的重量不会改变，状态的下一个估计值(预测值)等于当前的估计值:
x^2,1=x^1,1=1030g\hat{x}_{2,1} = \hat{x}_{1,1} =1030g x^2,1=x^1,1=1030g

第二次迭代

经过一个单位时间后，上一次迭代的预测值(predicted estimate)将成为当前迭代的先验估计值(previous estimate)：
x^2,1=1030g\hat{x}_{2,1} =1030g x^2,1=1030g

步骤1

测量。第二次称重：z2=989gz_2=989gz2=989g

步骤2

计算增益：α2=12\alpha_2 = \frac{1}{2}α2=21
计算当前估计值：x^2,2=x^2,1+α2(z2−x^2,1)=1030+12(989−1030)=1009.5g\begin{aligned} \hat{x}_{2,2} &= \hat{x}_{2,1} + \alpha_2 (z_2-\hat{x}_{2,1}) \\ &= 1030+\frac{1}{2}(989−1030) \\ &= 1009.5g \end{aligned}x^2,2=x^2,1+α2(z2−x^2,1)=1030+21(989−1030)=1009.5g
步骤3
x^3,2=x^2,2=1009.5g\hat{x}_{3,2} = \hat{x}_{2,2} =1009.5g x^3,2=x^2,2=1009.5g

第三次迭代
x^3,2=1009.5，z3=1017g，α3=13\hat{x}_{3,2}=1009.5，z_3 = 1017g，\alpha_3=\frac{1}{3} x^3,2=1009.5，z3=1017g，α3=31x^3,3=x^3,2+α3(z3−x^3,2)=1009.5+13(1017−1009.5)=1012g\begin{aligned} \hat{x}_{3,3} &= \hat{x}_{3,2} + \alpha_3 (z_3-\hat{x}_{3,2}) \\ &= 1009.5+\frac{1}{3}(1017−1009.5) \\ &= 1012g \end{aligned} x^3,3=x^3,2+α3(z3−x^3,2)=1009.5+31(1017−1009.5)=1012gx^4,3=x^3,3=1012g\hat{x}_{4,3} = \hat{x}_{3,3} = 1012g x^4,3=x^3,3=1012g

第四次迭代
x^4,3=1012g，z4=1009g，α4=14\hat{x}_{4,3}=1012g，z_4 = 1009g，\alpha_4=\frac{1}{4} x^4,3=1012g，z4=1009g，α4=41x^4,4=x^4,3+α4(z4−x^4,3)=1012+14(1009−1012)=1011.25g\begin{aligned} \hat{x}_{4,4} &= \hat{x}_{4,3} + \alpha_4 (z_4-\hat{x}_{4,3}) \\ &= 1012+\frac{1}{4}(1009−1012) \\ &= 1011.25g \end{aligned} x^4,4=x^4,3+α4(z4−x^4,3)=1012+41(1009−1012)=1011.25gx^5,4=x^4,4=1011.25g\hat{x}_{5,4} = \hat{x}_{4,4} = 1011.25g x^5,4=x^4,4=1011.25g

第五次迭代
x^5,4=1011.25g，z5=1013g，α5=15\hat{x}_{5,4}=1011.25g，z_5 = 1013g，\alpha_5=\frac{1}{5} x^5,4=1011.25g，z5=1013g，α5=51x^5,5=x^5,4+α5(z5−x^5,4)=1011.25+15(1013−1011.25)=1011.6g\begin{aligned} \hat{x}_{5,5} &= \hat{x}_{5,4} + \alpha_5 (z_5-\hat{x}_{5,4}) \\ &= 1011.25+\frac{1}{5}(1013−1011.25) \\ &= 1011.6g \end{aligned} x^5,5=x^5,4+α5(z5−x^5,4)=1011.25+51(1013−1011.25)=1011.6gx^6,5=x^5,5=1011.6g\hat{x}_{6,5} = \hat{x}_{5,5} = 1011.6g x^6,5=x^5,5=1011.6g

第六次迭代
x^6,5=1011.6g，z6=979g，α6=16\hat{x}_{6,5}=1011.6g，z_6 = 979g，\alpha_6=\frac{1}{6} x^6,5=1011.6g，z6=979g，α6=61x^6,6=x^6,5+α6(z6−x^6,5)=1011.6+16(979−1011.6)=1006.17g\begin{aligned} \hat{x}_{6,6} &= \hat{x}_{6,5} + \alpha_6 (z_6-\hat{x}_{6,5}) \\ &= 1011.6+\frac{1}{6}(979−1011.6) \\ &= 1006.17g \end{aligned} x^6,6=x^6,5+α6(z6−x^6,5)=1011.6+61(979−1011.6)=1006.17gx^7,6=x^6,6=1006.17g\hat{x}_{7,6} = \hat{x}_{6,6} = 1006.17g x^7,6=x^6,6=1006.17g

第七次迭代
x^7,6=1006.17g，z7=1008g，α7=17\hat{x}_{7,6}=1006.17g，z_7 = 1008g，\alpha_7=\frac{1}{7} x^7,6=1006.17g，z7=1008g，α7=71x^7,7=x^7,6+α7(z7−x^7,6)=1006.17+17(1008−1006.17)=1006.43g\begin{aligned} \hat{x}_{7,7} &= \hat{x}_{7,6} + \alpha_7 (z_7-\hat{x}_{7,6}) \\ &= 1006.17+\frac{1}{7}(1008-1006.17) \\ &= 1006.43g \end{aligned} x^7,7=x^7,6+α7(z7−x^7,6)=1006.17+71(1008−1006.17)=1006.43gx^8,7=x^7,7=1006.43g\hat{x}_{8,7} = \hat{x}_{7,7} = 1006.43g x^8,7=x^7,7=1006.43g

第八次迭代
x^8,7=1006.43g，z8=1042g，α8=18\hat{x}_{8,7}=1006.43g，z_8 = 1042g，\alpha_8=\frac{1}{8} x^8,7=1006.43g，z8=1042g，α8=81x^8,8=x^8,7+α8(z8−x^8,7)=1006.43+18(1042−1006.43)=1010.87g\begin{aligned} \hat{x}_{8,8} &= \hat{x}_{8,7} + \alpha_8 (z_8-\hat{x}_{8,7}) \\ &= 1006.43+\frac{1}{8}(1042-1006.43) \\ &= 1010.87g \end{aligned} x^8,8=x^8,7+α8(z8−x^8,7)=1006.43+81(1042−1006.43)=1010.87gx^9,8=x^8,8=1010.87g\hat{x}_{9,8} = \hat{x}_{8,8} = 1010.87g x^9,8=x^8,8=1010.87g

第九次迭代
x^9,8=1010.87g，z9=1012g，α9=19\hat{x}_{9,8}=1010.87g，z_9 = 1012g，\alpha_9=\frac{1}{9} x^9,8=1010.87g，z9=1012g，α9=91x^9,9=x^9,8+α9(z9−x^9,8)=1012+19(1012−1010.87)=1011g\begin{aligned} \hat{x}_{9,9} &= \hat{x}_{9,8} + \alpha_9 (z_9-\hat{x}_{9,8}) \\ &= 1012+\frac{1}{9}(1012-1010.87) \\ &= 1011g \end{aligned} x^9,9=x^9,8+α9(z9−x^9,8)=1012+91(1012−1010.87)=1011gx^10,9=x^9,9=1011g\hat{x}_{10,9} = \hat{x}_{9,9} = 1011g x^10,9=x^9,9=1011g

第十次迭代
x^10,9=1011g，z10=1011g，α10=110\hat{x}_{10,9}=1011g，z_{10} = 1011g，\alpha_{10}=\frac{1}{10} x^10,9=1011g，z10=1011g，α10=101x^10,10=x^10,9+α10(z10−x^10,9)=1011+110(1011−1011)=1011g\begin{aligned} \hat{x}_{10,10} &= \hat{x}_{10,9} + \alpha_{10} (z_{10}-\hat{x}_{10,9}) \\ &= 1011+\frac{1}{10}(1011-1011) \\ &= 1011g \end{aligned} x^10,10=x^10,9+α10(z10−x^10,9)=1011+101(1011−1011)=1011gx^11,10=x^10,10=1011g\hat{x}_{11,10} = \hat{x}_{10,10} = 1011g x^11,10=x^10,10=1011g

差不多可以了，增益随每次测量而减小，因此，每一个测量值的影响都比前一个测量值小。1010克很接近真实的重量了。如果进行更多的测量，就会更趋近真实值。

下表总结了测量值和估计值，图表比较了测量值、估计值和真实值。

nnn	1	2	3	4	5	6	7	8	9	10
αn\alpha_nαn	11\frac{1}{1}11	12\frac{1}{2}21	13\frac{1}{3}31	14\frac{1}{4}41	15\frac{1}{5}51	16\frac{1}{6}61	17\frac{1}{7}71	18\frac{1}{8}81	19\frac{1}{9}91	110\frac{1}{10}101
znz_nzn	1030	989	1017	1009	1013	979	1008	1042	1012	1011
x^n,n\hat{x}_{n,n}x^n,n	1030	1009.5	1012	1011.25	1011.6	1006.17	1006.43	1010.87	1011	1011
x^n+1,n\hat{x}_{n+1,n}x^n+1,n	1030	1009.5	1012	1011.25	1011.6	1006.17	1006.43	1010.87	1011	1011

如图所示，估计算法对测量值有平滑效果，估计值会趋近真实值。

总结

在这个例子中，我们为静态系统设计了一个简单的估计算法，并且推导出了卡尔曼滤波的五个方程之一的状态更新方程。