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

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

实际工程常常面临这样的情况：变量$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时刻估计状态为${\hat{X}}_{k-1}$，在k时刻有一个测量$Z_k$，那么k时刻对状态的预测${\hat{X}}_k$应该是多少呢？
回答这个问题，需要考察状态方程：
$$X_k={\Phi }_{k/k-1}{X}_{k-1}+{\Gamma }_{k-1}{W}_{k-1}$$
和量测方程：
$$Z_k=H_k{X}_k+V_k$$
以及必要的假设：系统噪声$W$和测量噪声$V$都是白噪声，即$E(W_k)=0$，$E(W_k{W}_j^T)=Q_k{\delta }_{kj}$，$E(V_k)=0$，$E(V_k{V}_j^T)=R_k{\delta }_{kj}$，并且假设k-1时刻状态估计为${\hat{X}}_{k-1}$，误差协方差为$P_{k-1}=E(X_{k-1}-{\hat{X}}_{k-1})(X_{k-1}-{\hat{X}}_{k-1}{)}^T$。
首先，既然在k-1时刻已经估计出状态为${\hat{X}}_{k-1}$，那么k时刻的状态按照状态方程就应当预测为： $${\hat{X}}_{k/k-1}={\Phi }_{k/k-1}{\hat{X}}_{k-1}$$ ${\hat{X}}_{k/k-1}$被称为状态一步预测值。
既然对k时刻状态做出预测，那么显然可以根据量测方程再对k时刻的测量值做出预测：$${\hat{Z}}_k=H_k{\hat{X}}_{k/k-1}$$。
既然对测量做出了预测，又获得了一个真正的测量值$Z_k$，那么显然它们的差$Z_k-{\hat{Z}}_k$反映了预测状态与真实状态的差。所以毫无疑问，需要利用这个差对状态做出修正：$${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$$
$K_k$是增益阵，那么剩下的问题就是如何选取$K_k$。选取$K_k$的原则是使得误差协方差阵 $E(X_k-{\hat{X}}_k)(X_k-{\hat{X}}_k{)}^T$最小，按照上式，对$K_k$的选取既要考虑${\hat{X}}_{k/k-1}$的误差协方差，也要考虑测量的噪声阵$R_k$，记$P_{k/k-1}$为${\hat{X}}_{k/k-1}$的误差协方差阵，则$K_k$为：$$K_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T+R_k{)}^{-1}$$
而$P_{k/k-1}$与上一步的误差协方差 $P_{k-1}$ 和系统噪声 $Q_{k-1}$有关：
$$P_{k/k-1}={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T$$
到这里就确定了增益K，给出了估计${\hat{X}}_k$，最后一步是计算 ${\hat{X}}_k$ 的误差协方差$P_k$：
$$P_k=[I-K_k{H}_k{]}^T{P}_{k/k-1}[I-K_k{H}_k{]}^T+K_k{R}_k{K}_k^T$$
$P_k$还有两种形式：
$$P_k=[I-K_k{H}_k]P_{k/k-1}$$$$P_k^{-1}=P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k$$
以及$K_k$也还有一种简单形式：$$K_k=P_k{H}_k^T{R}_k^{-1}$$

从上述过程可以看出，卡尔曼滤波遵循“预测-修正”的思想，通过对比测量真实值与测量预测值来修正状态估计。确定增益阵K是卡尔曼滤波的核心也是精髓所在，对增益阵的计算充分考虑了初始估计误差，系统噪声，测量噪声，给出了误差最小意义下的状态最优估计。

再说一下扩展卡尔曼（EKF）。
线性卡尔曼要求状态方程和量测方程都是线性方程，在实际问题中面临最多的却是非线性系统的问题。
对于非线性的状态方程和量测方程：
$$X_k=f(X_{k-1},k-1)+{\Gamma }_{k-1}{W}_{k-1}$$
$$Z_k=h(X_k,k)+V_k$$
上述的基于线性方程的做法已经不再适用了。为了能对非线性系统应用卡尔曼滤波，利用泰勒展开对原非线性系统在$X_{k-1}={\hat{X}}_{k-1}$ 处做一阶线性化：
$$X_k=f({\hat{X}}_{k-1},k-1)+J_f(k-1)(X_{k-1}-{\hat{X}}_{k-1})+{\Gamma }_{k-1}{W}_{k-1}$$
$$Z_k=h(X_{k-1},k-1)+J_h(k)(X_k-{\hat{X}}_{k-1})+V_k$$
这样就能应用线性方程中的做法进行滤波，这就是扩展卡尔曼滤波。
EKF在$X_k={\hat{X}}_k$处用一个线性化的方程对原非线性方程做近似，这个做法在系统非线性不强的时候能够给出比较好的估计，但是在非线性较强的时候，EKF很难获得较高的滤波精度。