一、一维卡尔曼滤波

通过8个数值例子介绍卡尔曼滤波，涉及平均数、方差、标准差等，最终可以自行设计出一个一维的卡尔曼滤波算法。

1.1 背景知识

平均值、期望值、方差、标准差

对于一个跟踪和控制系统，最大的问题计时在存在不确定性的前提下提供一个准确的有用信息。
卡尔曼滤波就是一种常用且重要的估算方法，它在预估时默认输入信息不准确，同时也根据上一次系统的预估值来预估下一次的系统状态，测量噪声和处理噪声的存在可能使得动态模型（描述输入与输出关系的方法）估算出来的结果与真实值相差甚远，因此需要对噪声进行处理。

平均值和期望值的区别在于，用可观测状态计算出的是均值，一般用 $\mu$ 表示，而对于无法获得真实值的隐变量，平均多次测量值估计的结果称为期望值，一般用 $E$ 表示。

方差和标准差，方差 Variance 用来衡量一组结果的离散程度，标准差 Standard Deviation 则是方差的算数平方根，两者分别用 $\sigma^2$ 和 $\sigma$ 表示。
方差、标准差在完全样本中的计算和在抽样样本中的计算是有区别的
在完全样本中：
$\sigma^2=\frac{1}{N}\sum^{N}_{n=1}(x_n-\mu)^2,\sigma=\sqrt{\frac{1}{N}\sum^{N}_{n=1}(x_n-\mu)^2}$
在抽样样本中：
$\sigma^2=\frac{1}{N-1}\sum^{N}_{n=1}(x_n-\mu)^2,\sigma=\sqrt{\frac{1}{N-1}\sum^{N}_{n=1}(x_n-\mu)^2}$

正太分布

很多自然现象都遵循正太分布，也称为高斯分布，可以通过以下等式描述：
$f(x;\mu,\sigma^2)=\frac{1}{\sqrt{2\pi\sigma^2}}e^{\frac{-(x-\mu)^2}{2\sigma^2}}$
高斯曲线也被称为正太分布的概率密度函数，如下图所示，以及正太分布在 $\mu\pm\sigma$ 、 $\mu\pm2\sigma$ 、 $\mu\pm3\sigma$ 的比例，通常，测量误差呈正太分布。

随机变量分为连续随机变量和离散随机变量，所有的测量值都是连续随机变量。

估计的准确度与精密度

估计用来估算系统的不可见状态，可以通过雷达等传感器估计飞机位置，并通过使用多个传感器和高级估计及追踪算法（如卡尔曼滤波）来显著提升估计精度。
准确度表示测量结果与真实值的接近程度。精密度表示测量结果的再现性。

随机误差导致方差，高/低精密度系统表示系统方差/不确定性的大小，而高/低精度系统表示系统系统性误差（偏差）的大小。对测量值进行平均或平滑处理可以显著降低方差影响，但是无法修复固定的系统误差，教程中案例都假设为无偏系统。

以一张图进行总结

1.2 $\alpha-\beta-\gamma$ 滤波器

静态模型案例-金条称重

估计一个静态模型的状态，
$\hat{x}_{n,n}=\frac{1}{n}(z_1+z_2+...+z_{n-1}+z_n)=\frac{1}{n}\sum^{n}_{i=1}(z_i)$

符号说明:
$x$ 是测量值
$z_n$ 是第 $n$ 次的测量值
$\hat{x}_{n,n}$ 是第 $n$ 次测量后的估计值
$\hat{x}_{n,n-1}$ 是 $n - 1$ 次测量后对 $n$ 时刻作出的先验估计值
$\hat{x}_{n+1,n}$ 是 $n$ 次测量后对 $n + 1$ 时刻作出的先验估计值
因为案例中的动态模型是定值，因此 $\hat{x}_{n+1,n}=\hat{x}_{n,n}$

经过计算可以得到 $\hat{x}_{n,n}=\hat{x}_{n-1,n-1}+\frac{1}{n}(z_n-\hat{x}_{n-1,n-1})$ ，这就是一个状态更新方程，可以将其总结为：
$当前状态估计=当前状态估计的先验估计值+Factor\times(测量值-当前状态估计的先验估计值)$

这里的 $F a c t or$ 在卡尔曼滤波中被称为卡尔曼增益，记为 $K_n$ ，这里暂以 $\alpha_n$ 代替 $K_n$ ，状态更性方程可以写为如下，其中的 $z_n-x_{n,n-1})$ 为观测残差/更新，包含了新的观测信息。
$\hat{x}_{n,n}=\hat{x}_{n,n-1}+\alpha_n(z_n-\hat{x}_{n,n-1})$
下面是整个估计过程：

一维空间追踪匀速飞行器

假设一个一维空间，一架飞行器向远离/接近雷达方向恒速飞行，雷达角度、飞机高度不变。 $x_n$ 表示飞行器在 $n$ 时的航程，雷达以恒定频率向目标发射追踪波束，周期为 $\Delta t$ ，可以用以下两个方程描述运动：
$x_{n+1}=x_n+\Delta t\dot{x}_n\\ \dot{x}_{n+1}=\dot{x}_n$
以上的方程组可以称为状态外推方程（State Exploration Equation）/过渡方程/预测方程，也是卡尔曼滤波的方程之一。
设定雷达跟踪周期 $\Delta t$ 为5秒，假设在 $n - 1$ 时无人机估计航程30000m，估计速度40m/s。可以外推预测 $n$ 时目标位置和速度值：
$\hat{x}_{n,n-1}=\hat{x}_{n-1,n-1}+\Delta t\hat{\dot{x}}_{n-1,n-1}=30000+5*40=30200m\\\ \\ \hat{\dot{x}}_{n,n-1}=\hat{\dot{x}}_{n-1,n-1}=40m/s$
如果在 $n$ 时雷达测量航程( $z_n$ )为30110，与预测值存在90m的偏差，可能有雷达测量误差、飞行器速度误差两方面因素导致.

下面写下飞行器速度的状态更新方程：
$\hat{\dot{x}}_{n}=\hat{\dot{x}}_{n,n-1}+\beta(\frac{z_n-\hat{x}_{n,n-1}}{\Delta t})$
这里的系数 $\beta$ 值取决于雷达精度：
（1）如果雷达精度高，倾向于是飞行器速度变化导致航程的测量偏差，则将 $\beta$ 调高。
（2）如果雷达精度低，倾向于是雷达测量误差导致航程的测量偏差，则将 $\beta$ 调低。

下面再写出飞行器位置的状态更新方程：
$\hat{x}_{n,n}=\hat{x}_{n,n-1}+\alpha(z_n-\hat{x}_{n,n-1})$
同样，这里的系数 $\alpha$ 值取决于雷达精度， $\alpha$ 在 0~1 之间， $\alpha=1$ ，则估计航程等于测量航程， $\alpha=0$ ，则测量没有意义。

由此得到雷达追踪器的状态更性方程组，也被称为 $\alpha-\beta$ 轨迹更新方程/ $\alpha-\beta$ 轨迹滤波方程。
$\alpha,\beta$ 的值取决于测量精度，在0～1之间，对精度越高的设备，测量权重越高，此时滤波器会对目标速度变化作出快速响应；而如果精度较低，则采用低 $\alpha、\beta$ ，降低测量中的不确定性。

注：有些教材中 $\alpha-\beta$ 滤波被写为 g-h 滤波。

一维空间中追踪加速飞行器

设定一架战斗机先以50m/s的恒定速度飞行15秒，然后以 $8 m/s^2$ 的加速度匀加速飞行35秒。

使用 $\alpha-\beta$ 滤波器进行追踪，通过每次周期的速度估计考虑加速度带来的影响，会看到真实值或测量值与估计值之间存在一个固定的差值，被称为滞后误差（lag error），它的其他常见名称还有：动态误差、系统误差、偏移误差、截断误差。

因此需要使用 $\alpha-\beta-\gamma$ 滤波器进行跟踪，将目标加速度考虑进去：

状态外推方程变为：
$\hat{x}_{n+1,n}=\hat{x}_{n,n}+\hat{\dot{x}}_{n,n}\Delta t+\hat{\ddot{x}}_{n,n}\frac{\Delta t^2}{2}\\\ \\ \hat{\dot{x}}_{n+1,n}=\hat{\dot{x}}_{n,n}+\hat{\ddot{x}}_{n,n}\Delta t\\\ \\ \hat{\ddot{x}}_{n+1,n}=\hat{\ddot{x}}_{n,n}$
状态更新方程变为
$\hat{x}_{n,n}=\hat{x}_{n,n-1}+\alpha(z_n-\hat{x}_{n,n-1})\\\ \\ \hat{\dot{x}}_{n,n}=\hat{\dot{x}}_{n,n-1}+\beta(\frac{z_n-\hat{x}_{n,n-1}}{\Delta t})\\\ \\ \hat{\ddot{x}}_{n,n}=\hat{\ddot{x}}_{n,n-1}+\gamma(\frac{z_n-\hat{x}_{n,n-1}}{0.5\Delta t^2})$
包含加速度模型的 $\alpha-\beta-\gamma$ 滤波器可以追踪匀加速运动的目标，并且消除滞后误差。