解读卡尔曼滤波

解读卡尔曼滤波

一：统计学的基础

1. 均值和期望值

   均值 Mean 和 期望值 Expected Value 是两个相似但不相同的概念。

   • 均值 Mean

     比方说我们有5枚硬币：2枚5分和3枚10分。我们很容易就能计算出它们的均值。

     $\mu =\frac{1}{N}{\sum }_{n=1}^N{V}_n=\frac{1}{5}\left(5+5+10+10+10\right)=8分$

   • 期望值 Expected Value

     现在比方说一个人测量了5次体重，5次的结果分别是：79.8kg，80kg，80.1kg，79.8kg，和80.2kg

     $E=\frac{1}{N}{\sum }_{n=1}^N{W}_n=\frac{1}{5}\left(79.8+80+80.1+79.8+80.2\right)=79.98kg$

2. 方差与标准差

   • 方差（字母 σ2） Variance ：：用来衡量一组数据的离散程度

     $${\sigma }^2=\frac{1}{N}\sum _{n=1}^N{\left(x_n-\mu \right)}^2$$

   • 标准差（字母 σ） ：：Standard Deviation 是方差的算数平方根

     $$\sigma =\sqrt[]{\frac{1}{N}\sum _{n=1}^N{\left(x_n-\mu \right)}^2}$$

3. 正态分布（高斯分布）
   

   • 分布密度区间：

     • 68.26%的值位于距均值的一个标准差内
     • 95.44%的值位于距均值的两个标准差内。
     • 99.74%的值位于距均值的三个标准差内。

   • 概率密度函数（PDF）

     $$f\left(x;\mu ,{\sigma }^2\right)=\frac{1}{\sqrt[]{2\pi {\sigma }^2}}{e}^{\frac{-{\left(x-\mu \right)}^2}{2{\sigma }^2}}$$

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

$x$ 是重量的真实值

$z_n$ 是第 $n$ 次称重的测量值

${\hat{x}}_{n,n}$ 是第 $n$ 次 $x$ 的估计值(测量出 $z_n$ 后进行估算)

${\hat{x}}_{n,n-1}$ 是 $x$ 的先验估计值，在时间 $n-1$ 时计算出(测量出 $z_{n-1}$ 后进行估算)

${\hat{x}}_{n+1,n}$ 是对未来 ($n+1$ ) 时 $x$ 的估计。在第 $n$次测量出 $z_n$ 后进行估算。换言之，${\hat{x}}_{n+1,n}$是一个预测值

1. 状态更新方程:卡尔曼滤波的第一个方程
   $${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+{\alpha }_n\left(z_n-{\hat{x}}_{n,n-1}\right)$$

2. 状态外推方程State Extrapolation Equation:: 卡尔曼滤波的第二个方程
   $$x_{n+1}=x_n+\Delta t{\dot{x}}_n$$
   $$dot{x}_{n+1}={\dot{x}}_n$$

三：一维卡尔曼滤波器

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

1. 测量误差（measurement uncertainty）：r

2. 估计误差（estimate uncertainty）：p

3. 卡尔曼增益方程:卡尔曼滤波的第三个器方程

   $$K_n=\frac{UncertaintyinEstimate}{UncertaintyinEstimate+UncertaintyinMeasurement}=\frac{p_{n,n-1}}{p_{n,n-1}+r_n}$$

   $p_{n,n-1}$ is the extrapolated estimate uncertainty
   $r_n$ is the measurement uncertainty

   • 卡尔曼增益是一个介于 0 和 1 之间的数字：

     $0\le K_n\le 1$

   • 卡尔曼增益告诉我们

4. 协方差更新方程 : 卡尔曼滤波的第四个器方程

   • 测量误差远大于估计误差，卡尔曼增益会很低，因此估计不确定度的收敛会很慢。
   • 估计误差远大于测量误差，卡尔曼增益会很高，估计不确定度会很快收敛到零。

   $$p_{n,n}=\left(1-K_n\right)p_{n,n-1}$$

   $K_n$ 是卡尔曼增益

   $p_{n,n-1}$ 是在先前滤波器估计期间计算的估计不确定性

   $p_{n,n}$ 是当前状态的估计不确定性

5. 协方差外推方程：估计不确定性外推方程

   $$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$$

   $p^x$ 是位置估计不确定性

   $p^v$ 是速度估计的不确定性

6. 卡尔曼滤波流程图