mpu6050姿态解算与卡尔曼滤波（2）卡尔曼滤波

卡尔曼滤波，是最优估计理论中十分重要的一个部分。
要全面地理解卡尔曼滤波，你需要一点统计学的知识，以及一点矩阵理论。通常最优估计理论的教材是从最小二乘估计讲起，接着讲到最小方差估计，极大似然估计以及维纳滤波，再到卡尔曼滤波、扩展卡尔曼、无迹卡尔曼-------这是从统计的角度来理解卡尔曼滤波。
事实上，卡尔曼滤波与《自动控制原理》中的状态观测器非常相似。状态观测器利用原系统的输入输出来估计系统状态，对原系统建模(状态方程，输出方程)，把观测器的输出与原系统输出的差乘上一个增益反馈给观测器的状态，这样构成一个闭环系统使得观测器状态跟踪原系统状态。卡尔曼滤波中，同样要对系统建模得到状态方程，输出方程则取决于你的测量手段，因此改称量测方程。卡尔曼同样要预测量测值，并把预测值与真实量测值比较，乘上一个增益返回给状态。不同之处在于，状态观测器不考虑噪声，要求系统可检测，并且增益通常是时不变的，而卡尔曼滤波则详细考虑了噪声和初始估计误差，智能的计算每一步的增益，以达到对状态的最优的估计。

---

实际工程常常面临这样的情况：变量$X$无法直接测量，能测量的是一些与$X$相关的量$Z$，通常可以得到确定的函数关系：$Z=H(X)$，例如要用mpu6050测量转动欧拉角$(\psi ,\theta ,\gamma {)}^T$，无法直接测量，但是加速度计的测量值与欧拉角有关系：
$$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[\begin{array}{c}-g\sin \gamma \cos \theta \\ g\sin \theta \\ g\cos \gamma \cos \theta \end{array}\right]$$
由测量值Z估计状态X，典型的方法–最小二乘估计，即对X的估计—$\hat{X}$应当使得$E\{(Z-H(\hat{X}))(Z-H(\hat{X}){)}^T\}$最小。典型例子如用直尺测量一支笔的长度，多次测量取平均值就是一个最简单的最小二乘估计。
实际工程中，对状态$X$可以通过建模，给出$X$的动力学过程：$\dot{X}=f(X,t)$，若能确定初始时刻的状态$X_0$，那么显然初始时刻以后的状态都可以确定，对状态$X$的确定就是一个微分方程求解的问题。然而实际系统$f(X)$多具有复杂的形式，给出$X$的解析解是一件困难甚至不可能的事。好在我们并不要求$X$绝对精确的值，只需要满足精度需求的值，那么我们可以依靠数值积分方法给出状态$X$的解，只要步长$\Delta t$取得足够小，龙格库塔法正是其中的一种典型方法。
$X$的动力学过程与对$Z$的测量都包含有关于$X$的信息，卡尔曼滤波正是综合运用了这两种信息来给出状态x的最优估计。
卡尔曼滤波包含两个基本方程–状态方程和量测方程，这也是卡尔曼滤波最根本的地方，但凡要实现一个卡尔曼滤波器，只要写出状态方程和量测方程，剩下的就只是代公式了，如果连状态方程和测量方程都写不出来搞不清楚，那也别谈什么实现卡尔曼滤波器。
状态方程：$$X_k={\Phi }_{k/k-1}{X}_{k-1}+{\Gamma }_{k-1}{W}_{k-1}$$
状态方程描述了状态$X$的动力学过程，${\Phi }_{k/k-1}$是从k-1时刻到k时刻的状态转移矩阵，${\Gamma }_{k-1}$是k-1时刻的噪声输入矩阵，$W_{k-1}$是k-1时刻的系统噪声。
量测方程：$$Z_k=H_k{X}_k+V_k$$
量测方程描述了状态与量测量的关系，$V_k$是k时刻的测量噪声。
上述两个方程是标准线性卡尔曼的基本方程，它要求两个方程是线性的，还要求系统噪声$W$和测量噪声$V$都是白噪声。
卡尔曼的推导过程需要较强的数学基础，这里不讨论，只说明卡尔曼的具体做法和物理意义。
卡尔曼要解决的问题：若在k-1时刻估计状态为X^k−1\hat X_{k-1}