卡尔曼录波器 :2. Fata Fusion, covarince Matrix, state space, observation

卡尔曼录波器 :2. Fata Fusion, covarince Matrix, state space, observation

卡尔曼滤波器 数据融合 协方差矩阵状态空间方程 观测器

Data Fusion 数据融合

两个或者多个测量仪器 ，两者都会存在着测量误差，测量的标准差是服从正态分布的
$$\begin{gathered} z_1=30g\sigma =2g \\ {z}_2=32g\sigma =4g \end{gathered}$$
根据上面的测量值，估计真实值$\hat{z}=?$

可以采用卡尔曼的思想设计
$$\hat{z}=z_1+K(z_2-z_1)k[0,1]\text{\hspace{1em}(1)}$$
可以看出来
$$\begin{gathered} k=0:\hat{z}=z_1 \\ k=1:\hat{z}=z_2 \end{gathered}$$
求解合适的K使得${\sigma }_z$最小，$var(\hat{z})$最小,其中因为两个测量工具是相互独立的因此$z_1,z_2$也是相互独立的
$$\begin{array}{rl}{\sigma }_z^2& =var(z_1+K(z_2-z_1))\\ & =var(z_1-K{z}_1+K{z}_2)\\ & =var((1-K)z_1+K{z}_2)\\ & =var((1-K)z_1)+var(K{z}_2)\\ & =(1-k{)}^{2}var(z_1)+K^{2}var(z_2)\\ & =(1-K{)}^2{\sigma }_1^2+K^2{\sigma }_2^2\end{array}$$
为了求得${\sigma }_z$最小，对K求导
$$\begin{gathered} \frac{d{\sigma }_z^2}{dK}=0 \\ -2(1-K){\sigma }_1^2+2K{\sigma }_2^2=0 \\ K({\sigma }_1^2+{\sigma }_2^2)={\sigma }_1^2 \\ K=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}=0.2 \end{gathered}$$
将$K$带入到(1)式中，就可以把最优的估计值求解出来，即
$$\begin{array}{rl}& 估计值：\hat{z}=z_1+K(z_2-z_1)=30+0.2(32-30)=30.4\\ & 方差：{\sigma }_z^2=(1-0.2^2)2^2+0.2^2{4}^2=3.2\end{array}$$
以上的过程就是数据融合

协方差矩阵

协方差矩阵就是把方差和协方差在一个矩阵中表现出来，突出了变量之间的联系

举个例子：

球员	身高$x$	体重$y$	年龄$z$
瓦尔迪	179	74	33
奥巴梅扬	187	80	31
萨拉赫	175	71	28
平均	180.3	75	30.7

求解方差：
$$\begin{array}{rl}方差：& {\sigma }_x^2=\frac{1}{3}((179-180.3{)}^2+(187-180.3{)}^2+(175-180.3{)}^2)=24.89\\ & {\sigma }_y^2=\frac{1}{3}((74-75{)}^2+(80-75{)}^2+(71-75{)}^2)=14\\ & {\sigma }_z^2=\frac{1}{3}((33-30.7{)}^2+(31-30.7{)}^2+(28-30.7{)}^2)=4.22\\ 协方差：& {\sigma }_x{\sigma }_y=\frac{1}{3}((179-180.3)(74-75)+(187-180.3)(80-75)+(175-180.3)(71-75))=18.7={\sigma }_y{\sigma }_x\\ & {\sigma }_x{\sigma }_z={\sigma }_z{\sigma }_x=4.4\\ & {\sigma }_y{\sigma }_z={\sigma }_z{\sigma }_y=3.3\end{array}$$
协方差矩阵的形式：
KaTeX parse error: Undefined control sequence: \matrix at position 11: p=\left[ \̲m̲a̲t̲r̲i̲x̲{ \sigma^2_x&\s…
如何利用矩阵的变换计算协方差？编程中有可能用到

首先我们要引入一个过渡矩阵
KaTeX parse error: Undefined control sequence: \matrix at position 13: a = \left[ \̲m̲a̲t̲r̲i̲x̲{ x_1&y_1&z_1\\…

$$P=\frac{1}{3}{a}^{T}a$$
协方差矩阵说明了什么？说明两个变量之间的关系

状态空间 state space

现代控制理论就是以状态空间

弹簧阻尼系统

$$\begin{gathered} m\ddot{x}+B\dot{x}+kx=F \\ m\ddot{x}+B\dot{x}+kx=uu:input \end{gathered}$$
状态变量state:
$$
\begin{align}\label{2}
使得：x_1 &= x\
x_2 &= \dot{x}\
这样：\dot{x}_1 &= x_2\
\dot{x}_2 &= \ddot{x} = \frac{1}{m}u-\frac{B}{m}\dot{x}-\frac{k}{m}x\
&=\frac{1}{m}u-\frac{B}{m}x_2-\frac{1}{m}x_1

\end{align}
$$
这样就用两个一阶方程把状态表示出来了

测量（measurement）

$$\begin{array}{r}{z}_1=x=x_1位置\\ z_2=\dot{x}=x_2速度\end{array}$$
把上述的式子用矩阵的形式进行表达
KaTeX parse error: Undefined control sequence: \matrix at position 8: \left[\̲m̲a̲t̲r̲i̲x̲{\dot{x}_1\\\do…
归纳一下就是（以下是一种连续的表达形式）
$$\begin{array}{rl}{\dot{X}}_t& =A{X}_t+B{u}_t\\ Z_t& =H{X}_t\end{array}$$
体现了随着时间的变化

如果离散的话
$$\begin{array}{rl}{X}_k& =A{X}_{k-1}+B{u}_k\\ Z_k& =H{X}_k\end{array}$$

下标$k,k-1,k_1$时间单位代表的是采样时间，不是从连续系统下直接抄过来，需要根据采样时间进行调整

因为系统存在着很多不确定性，因此把上述式子变成：

过程噪声：process noise ${\omega }_{k-1}$

测量噪声：Measurement noise ${\nu }_k$
$$\begin{array}{rl}{X}_k& =A{X}_{k-1}+B{u}_k+{\omega }_{k-1}\\ Z_k& =H{X}_k+{\nu }_k\end{array}$$
在这两个都不确定的情况下，如何去估计一个精确的${\hat{x}}_k=?$

就是卡尔曼滤波器要解决的问题。。。

结合数据融合的例子，就是手上有一个不太准的测量结果，一个不太准的估计结果，找到一个比他们两个误差都要小的结果，这就是卡尔曼的一个应用实例