Note-卡尔曼滤波（一）

卡尔曼滤波（Kalman Filter）

Optimal Recursive（递归） Data Processing Algorithm
$$\overline{x_k}={\overline{x}}_{k-1}+k_k(z_k-{\overline{x}}_{k-1})$$

• 当前的估计值=上一次的估计值+系数（当前测量值-上一次估计值）

$$\begin{gathered} k_k:kalmanQain卡尔曼增益 \\ {e}_{EST}:估计误差 \\ {e}_{MEA}:测量误差 \\ {k}_k=\frac{e_{EST(k-1)}}{e_{EST(k-1)}+e_{MEA(k)}} \end{gathered}$$

$$\overline{x_k}={\overline{x}}_{k-1}+k_k(z_k-{\overline{x}}_{k-1})$$

$$e_{EST(k)}=(1-k_k)e_{EST(k-1)}$$

$$\begin{gathered} e_{EST(K-1)}>>e_{MEA(k)}:则k_k\to 1\overline{x_k}={\overline{x}}_{k-1}+k_k(z_k-{\overline{x}}_{k-1}) \\ {e}_{EST(K-1)}<<e_{MEA(k)}:则k_k\to 0\overline{x_k}={\overline{x}}_{k-1} \end{gathered}$$

• 计算步骤
  • 算当前的k_k
  • 算x_k
  • 算当前e_EST(k)

数据融合(Data Fusion) 协方差矩阵(Covariance Matrix) 状态空间方程(state Space) 观测器(observation)

• 数据融合

  现在有数据
  $$\begin{gathered} z_1=30g,{\delta }_1=2g \\ {z}_2=32g,{\delta }_1=4g \\ 求\overline{z}和\delta \end{gathered}$$
  由前面的公式我们有：
  $$\overline{z_k}={\overline{z}}_{k-1}+k_k(z_k-{\overline{z}}_{k-1})$$
  即：
  $$\begin{gathered} z=z_1+k(z_2-z_1) \\ {\delta }^{2}z=D(z)=D(z_1+k(z_2-z_1))=D((1-k)z_1+k{z}_2)=(1-k{)}^2{\delta }_1^2+k^2{\delta }_2^2 \end{gathered}$$
  对k求导取最小值
  $$k=\frac{{\delta }_1^2}{{\delta }_1^2+{\delta }_2^2}$$

• 协方差矩阵
  $$\begin{gathered} {\delta }_x^2,{\delta }_x{\delta }_y,{\delta }_x{\delta }_z \\ {\delta }_y{\delta }_x,{\delta }_y^2,{\delta }_y{\delta }_z \\ {\delta }_z{\delta }_x,{\delta }_z{\delta }_y,{\delta }_z^2 \end{gathered}$$
  快速算法：
  $$\begin{gathered} a=A-\frac{1}{3}\overline{A} \\ P=\frac{1}{3}{a}^{T}a \end{gathered}$$

• 状态空间方程

离散型：
$$\begin{gathered} X_k=A{X}_{k-1}+B{u}_k+w_{k-1} \\ {Z}_k=H{X}_k+v \\ {w}_{k-1}:过程噪音 \\ {v}_k:测量噪音 \end{gathered}$$