参考视频

递归算法

Optimal Recursive Data Processing Algorithm
最优化 递归 数据 处理 算法

不确定性：

  • 不存在完美的数学模型
  • 系统的扰动不可控，也很难建模
  • 测量的传感器存在误差

例：

用一把尺子测量硬币的直径。假设$Z_k$为测量结果 , 下标$k$为测量次数 , 多次测量后得到
$$\left\{
\begin{array}{ll}
Z_1 = 50.1mm \\
Z_2 = 50.3mm \\
Z_3 = 50.2mm \\
\end{array}
\right. $$

$估计(estimate) , 即测量数据 \xrightarrow{} \text{取平均值}$

$$
\begin{array}{ll}
估计值\widehat{X}_k & = \frac{1}{k}(Z_1 + Z_2 + Z_3 + … + Z_k) \\
& = \frac{1}{k}(Z_1 + Z_2 + Z_3 + … + Z_{k-1}) + \frac{1}{k}Z_k \\
& = \frac{1}{k} * \frac{k-1}{k-1} *(Z_1 + Z_2 + Z_3 + … + Z_{k-1}) + \frac{1}{k}Z_k \\
\xrightarrow{此时括号部分与分母(k-1)为k-1项的平均值} & = \frac{k-1}{k} \widehat{X}_{k-1} + \frac{1}{k}Z_k \\
\xrightarrow{乘以分子} & = \widehat{X}_{k-1} - \frac{1}{k} \widehat{X}_{k-1} + \frac{1}{k}Z_k \\
\xrightarrow{整理得到} \widehat{X}_k & = \widehat{X}_{k-1} + \frac{1}{k} ( Z_k - \widehat{X}_{k-1}) \\
\end{array}
$$

分析得到的公式 , 随着k增大 , $\frac{1}{k} \xrightarrow{趋近于} 0 $ . $\widehat{X}_k \xrightarrow{趋近于} \widehat{X}_{k-1}$
即:: 随着k的增加 , 测量结果不再重要

令$\frac{1}{k} = K_k$ , 则有

$$
\widehat{X}_ k = \widehat{X}_ {k-1} + K_k ( Z_k - \widehat{X}_ {k-1} )
$$
$ \xrightarrow{} $ 当前估计值 = 上一次的估计值 + 系数 * (当前测量值 - 上一次的估计值)

  • 在卡尔曼滤波中,$K_k$叫做卡尔曼增益(卡尔曼因数)
  • 从公式看出 , 当前估计值与上次估计值有关 , 而上次估计值与上上次估计值有关 . 这就是递归的思想

系数$k_k$

  • $k_k \in [0,1] $
  • 估计误差 : 用$e_{EST}$表示 . e表示Error误差 . EST 表示Estimate 估计 .
  • 测量误差 : 用$e_{MEA}$表示 . MEA表示Measurement 测量

此时

$$ K_k = \frac{ {e_{EST} }_ {k-1} }{ {e_ {EST} }_ {k-1} + {e_{MEA} }_k} $$
此公式为卡尔曼滤波的核心公式

另: ${e_ {EST} }_ k = (1 - K_ k){e_ {EST} }_ {k-1} $

讨论 , 在k时刻 :

  • ${e_ {EST} }_ k >> {e_ {MEA} }_ k$ , 即估计误差远大于测量误差 , 此时$K_ k \xrightarrow{趋近于}1$ .
    $$\widehat{X}_ k = {\widehat{X} }_ {k-1} + Z_ k - {\widehat{X} }_ {k-1} \ = Z_ k$$
    (即 : 估计误差很大,我们相信测量值)
  • ${e_ {EST} }_ k << {e_ {MEA} }_ k$ , 即估计误差远小于测量误差 , 此时$K_ k \xrightarrow{趋近于}0$ .
    $$\widehat{X}_ k = {\widehat{X} }_ {k-1} $$
    (即 :测量误差很大,我们相信估计值)

例：

  • Step1: 计算 Kalman Gain $K_ k = \frac{ {e_ {EST} }_ {k-1} }{ {e_ {EST} }_ {k-1} + {e_ {MEA} }_ k}$
  • Step2: 计算当前估计值 $\widehat{X}_ k = \widehat{X}_ {k-1} + K_ k ( Z_ k - \widehat{X}_ {k-1}) $
  • Step3: 更新当前估计误差 ${e_ {EST} }_ k = (1 - K_ k){e_ {EST} }_ {k-1}$

$$\begin{array}{xx}
实际长度 x & = 50mm \\
估计值 \widehat{x}_ 0 & = 40mm \\
估计误差 {e_ {EST} }_ 0 & = 5mm \\
测量值 Z_1 & = 51mm \\
测量误差 {e_ {MEA} }_ k & = 3mm \\
\end{array}
$$

则有
$$
\begin{array}{c|lcr}
k & \ Z_ k & \ {e_ {MEA} }_ k & \ \widehat{x}_ k & \ K_ k & \ {e_ {EST} }_ k \\
\hline
0 & & & 40 & & 5 \\
1 & 51 & 3 & 46.875 & 0.625 & 1.875 \\
2 & 48 & 3 \\
3 & 49 & 3 \\
\end{array}
$$

$$\begin{align}{ll}
k = 1时 \xrightarrow{} K_ k & = \frac{5}{5+3} = 0.625 \\
\widehat{x}_ k & = 40 + 0.625(51 - 40) = 46.875 \\
{e_ {EST} }_ k & = (1 - 0.625)5 = 1.875 \\
\end{align}
$$
以此类推 , 在算到k = 3时 , ${\widehat{x} }_k \approx 50$

数据融合\协方差矩阵\状态空间方程\观测器

数据融合 Data Fusion

例：

两个都不准的测量器测量一个东西 , 分别得到两个结果 . 测量的结果都符合正态分布(高斯分布)

$$
\begin{array}{c|lcr}
不准的称 & \ 测量值 & \ 标准差σ & \\
\hline
称1 & 30g & 2g \\
称2 & 32g & 4g \\
\end{array}
$$
落在标准差范围内的概率为68.4%
(标准差越大,正态分布图像越宽且越矮小)

估算真实值
$$\widehat{Z} = Z_1 + k(Z_2 - Z_1) , k \in [0,1]\\
k = 0 \xrightarrow{} \widehat{Z} = Z_1 \\
k = 1 \xrightarrow{} \widehat{Z} = Z_2
$$

求k使得$σ_{\widehat{Z} } $最小 $\xrightarrow{}$ 方差 $Var(\widehat{Z})$ 最小

$$ \begin{align}
σ^2_{\widehat{Z} }
& = Var(Z_1 + k(Z_2 - Z_1)) \\
& = Var(Z_1 - kZ1 + kZ_2) \\
& = Var((1-k)Z_1 + kZ_2) \xrightarrow{} 可以看出Z_1,Z_2是相互独立的 \\
& = Var((1-k)Z_1) + Var(kZ_2) (方差的性质)\\
& = (1-k)^2Var(Z_1) + k^2Var(Z_2) \\
& = (1-k)^2σ^2_1 + k^2σ^2_2
\end{align}
$$

对其求导,就可求得极值
$$
\begin{align}
\frac{dσ^2_{\widehat{Z} } }{dk} & = 0 \\
\xrightarrow{} -2(1-k)σ^2_1 + 2kσ^2_2 & = 0 \\
-(1-k)σ^2_1 + kσ^2_2 & = 0 \\
-σ^2_1 + kσ^2_1 + kσ^2_2 & = 0 \\
\xrightarrow{} k = \frac{σ^2_1}{σ^2_1 + σ^2_2} & = \frac{2^2}{2^2 + 4^2} = 0.2
\end{align}
$$

得到k后代入计算最后结果 $$\widehat{Z} = Z_1 + k(Z_2 - Z_1) = 30 + 0.2(32 - 30) = 30.4$$

即根据称的特性与测量结果做出的预测为30.4g

协方差矩阵 Covariance Matrix

将方差与协方差在一个矩阵中表现出来 , 表现了变量间的联动关系

例

$$
\begin{array}{c|lcr}
球员& \ 身高x& \ 体重y & \ 年龄z \\
\hline
A & 179 & 74 & 33 \\
B & 187 & 80 & 31 \\
C & 175 & 71 & 28 \\
平均 & 180.3 & 75 & 30.7 \\
\end{array}\\
$$
方差 :
$$σ^2_x = \frac{1}{3}((179-180.3)^2 + (187-180.3)^2 + (175-180.3)^2) = 24.89 \\
σ^2_y = 14 \\
σ^2_z = 4.22 \\$$
协方差矩阵:
$$
σ_xσ_y = \frac{1}{3}((179-180.3)(74-75) + (187-180.3)(80-75) + (175-180.3)(71-75)) = 18.7 = σ_yσ_x \\
σ_xσ_z = 4.4 = σ_zσ_x \\
σ_yσ_z = 3.3 = σ_zσ_y \\
P=\left[\begin{matrix}
σ^2_x & σ_xσ_y & σ_xσ_z \\
σ_yσ_x & σ^2_y & σ_yσ_z \\
σ_zσ_x & σ_yσ_z & σ^2_z \\
\end{matrix}
\right]
= \left[\begin{matrix}
24.89 & 18.7 & 4.4 \\
18.7 & 14 & 3.3 \\
4.4 & 3.3 & 4.22 \\
\end{matrix}\right]
$$

状态空间方程 State Space Equation

状态空间方程 : 状态空间方程是描述状态变量与观测变量之间的关系的数学方程式。

测量\计算中充满了不确定性 . 所以可以在其中加入噪音参数
$X_{计算结果} = AX_{k-1}(上一时刻结果) + Bu_{k测量结果} + w_{k-1}(过程噪声)$
$Z_{测量结果} = HX_{k} + v_{k}(测量噪声)$
对 $X_{计算结果}$ 与$Z_{测量结果}$ 进行融合可使得结果更精确