一维卡尔曼滤波

卡尔曼滤波

基本概念

均值 $\mu$
期望值 $E$
方差 ${\sigma }^2$
$${\sigma }^2=\frac{1}{N-1}\sum _{n=1}^N{\left(x_n-\mu \right)}^2$$
标准差 $\sigma$
$$\sigma =\sqrt{\frac{1}{N-1}\sum _{n=1}^N{\left(x_n-\mu \right)}^2}$$
高斯分布
$$f\left(x;\mu ,{\sigma }^2\right)=\frac{1}{\sqrt{2\pi {\sigma }^2}}{e}^{\frac{-(x-\mu {)}^2}{2{\sigma }^2}}$$

测量噪声/测量误差/测量不确定度: 雷达系统的测量数值不是完全可靠，它包含随机误差(或者这类型的不确定性)。例如雷达自身的准确性，发射光束的宽度，返回信号强弱等等
处理噪声: 有很多外部因素会做成干扰，目标运动并不是完全按着运动方程。例如：风向，空气流动，驾驶策略等等
估计: 用来估算系统的不可见状态。测量或计算出的的参数都是估计值
准确度: 表示测量值与真实值的接近程度
精密度: 表示测量结果的再现性
测量值: 是一个由概率密度函数描述的随机变量
测量值的均值: 随机变量的期望值
测量值的均值和真实值的差: 偏差/系统误差/测量精度
分布的离散程度: 测量值的精密度/测量噪声/随机测量误差/测量不确定性
静态系统: 在相当一段时间内不改变状态的系统

$x$: 是重量的真实值
$z_n$: 是第 $n$ 次称重的测量值
${\hat{x}}_{n,n}$: 是第 $n$ 次 $x$ 的估计值(测量出 $z_n$ 后进行估算)
${\hat{x}}_{n,n-1}$: 是 $x$ 的先验估计值， 在时间 $n-1$ 时计算出
${\hat{x}}_{n+1,n}$: 是对未来 $(n+1)$ 时 $x$ 的估计。

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

例子1 – 金条重量(静态系统)

静态系统/状态外推方程:
$$x_{n+1,n}=x_{n,n}$$
推导:
$$\begin{array}{rl}{\hat{x}}_{N,N}& =\frac{1}{N}\sum _{n=1}^N\left(z_n\right)\\ & =\frac{1}{N}\left(\sum _{n=1}^{N-1}\left(z_n\right)+z_N\right)\\ & =\frac{1}{N}\frac{N-1}{N-1}\sum _{n=1}^{N-1}\left(z_n\right)+\frac{1}{N}{z}_N\\ & =\frac{N-1}{N}\frac{1}{N-1}\sum _{n=1}^{N-1}\left(z_n\right)+\frac{1}{N}{z}_N\\ & =\frac{N-1}{N}{\hat{x}}_{N-1,N-1}+\frac{1}{N}{z}_N\\ & ={\hat{x}}_{N-1,N-1}-\frac{1}{N}{\hat{x}}_{N-1,N-1}+\frac{1}{N}{z}_N\\ & ={\hat{x}}_{N-1,N-1}+\frac{1}{N}\left(z_N-{\hat{x}}_{N-1,N-1}\right)\end{array}$$
状态更新方程:
$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+K_n\left(z_n-{\hat{x}}_{n,n-1}\right)$$
卡尔曼增益: $K_n$
观测残差: $\left(z_n-x_{n,n-1}\right)$
初始猜测: 也就是第一个估计值

例子2 - 匀速飞行器

动态模型/状态外推方程/过渡方程/预测方程:
$$\begin{array}{rl}{x}_{n+1}& =x_n+\Delta t{\dot{x}}_n\\ {\dot{x}}_{n+1}& ={\dot{x}}_n\end{array}$$

速度的状态更新方程:
$${\hat{\dot{x}}}_{n,n}={\hat{\dot{x}}}_{n,n-1}+\beta \left(\frac{z_n-{\hat{x}}_{n,n-1}}{\Delta t}\right)$$
位置的状态更新方程:
$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+\alpha \left(z_n-{\hat{x}}_{n,n-1}\right)$$
$\alpha$ 和 $\beta$ 的值取决于测量精度

例子3-匀加速直线运动

滞后误差/动态误差/系统误差/偏移误差/截断误差: 真实值或测量值与估计值之间存在一个固定的差值。

状态外推方程:
$$\begin{array}{rl}{\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}\end{array}$$

状态更新方程:
$$\begin{array}{rl}{\hat{x}}_{n,n}& ={\hat{x}}_{n,n-1}+\alpha \left(z_n-{\hat{x}}_{n,n-1}\right)\\ {\hat{\dot{x}}}_{n,n}& ={\hat{\dot{x}}}_{n,n-1}+\beta \left(\frac{z_n-{\hat{x}}_{n,n-1}}{\Delta t}\right)\\ {\hat{\ddot{x}}}_{n,n}& ={\hat{\ddot{x}}}_{n,n-1}+\gamma \left(\frac{z_n-{\hat{x}}_{n,n-1}}{0.5\Delta t^2}\right)\end{array}$$

估计误差: 目标可以通过转弯突然改变飞行方向，真实的目标动态模型可能包括一个突然加速（改变加速度）

常见的 $\alpha -\beta -(\gamma )$ 滤波器

• 最小二乘法滤波
• 卡尔曼滤波
• 拓展卡尔曼滤波
• 无迹卡尔曼滤波

一维卡尔曼滤波器

无过程噪声的一维卡尔曼滤波器

测量不确定度/测量误差: $\gamma ={\sigma }^2$
要么被厂商提供，要么通过校准程序得出
估计不确定度/估计误差: $p$
但是可以评估出估计不确定度

卡尔曼增益方程

$$\begin{gathered} K_n=\frac{\text{ Uncertainty in Estimate }}{\text{ Uncertainty in Estimate }+\text{ Uncertainty in Measurement }}=\frac{p_{n,n-1}}{p_{n,n-1}+r_n} \\ {p}_{n,n-1}是外推估计不确定度 \\ {r}_n是测量不确定度 \end{gathered}$$

状态更新方程修改为:
$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+K_n\left(z_n-{\hat{x}}_{n,n-1}\right)=\left(1-K_n\right){\hat{x}}_{n,n-1}+K_n{z}_n$$
$k_n$ 是测量的权重，$(1-K_n)$ 是估计的权重。

协方差更新方程

$$\begin{gathered} p_{n,n}=\left(1-K_n\right)p_{n,n-1} \\ {p}_{n,n-1}是在先前滤波器估计期间计算的估计不确定性 \\ {p}_{n,n}是当前状态的估计不确定性 \end{gathered}$$

协方差外推方程

静态系统:
$$p_{n,n}=\left(1-K_n\right)p_{n,n-1}$$
匀速直线运动:
$$\begin{array}{rl}{p}_{n+1,n}^x& =p_{n,n}^x+\Delta t^2\cdot p_{n,n}^v\\ p_{n+1,n}^v& =p_{n,n}^v\end{array}$$

卡尔曼滤波过程

1. 初始化
   初始系统状态 $\left({\hat{x}}_{0,0}\right)$
   初始估计不确定度 $\left(p_{0,0}\right)$
2. 测量
   测量系统状态 $z_n$
   测量不确定度 $r_n$
3. 状态更新
   卡尔曼增益 $K_n$
   当前系统状态估计值 $\left({\hat{x}}_{n,n}\right)$
   当前状态估计不确定度 $\left(p_{n,n}\right)$
4. 预测
   下一个系统状态 $\left({\hat{x}}_{n+1,n}\right)$
   下一个估计不确定度 $\left(p_{n+1,n}\right)$

有过程噪声的的一维卡尔曼滤波器

过程噪声: 动态模型的不确定性，过程噪声会产生估计误差
过程噪声方差: $q$
协方差外推公式:
$$p_{n+1,n}=p_{n,n}+q_n$$

可以解决滞后误差通过设置一个高的过程不确定性