{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# MPU6050位姿解算简述"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 一、姿态角\n",
    "\n",
    "   所谓姿态角，就是机体飞行时相对于世界坐标系下的姿态角，也即是三方向欧拉角的统称，包含：滚转角roll、俯仰角pitch、偏航角yaw。  \n",
    "姿态角的定义：  \n",
    "1. 滚转角Φ（roll）：机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。\n",
    "![roll](http://www.123kuai.com/uploadfile/2014/0114/0_1303867652XD88.gif)\n",
    "2. 俯仰角θ（pitch）：机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。\n",
    "![pitch](http://www.123kuai.com/uploadfile/2014/0114/0_13038676358dsT.gif)\n",
    "3. 偏航角ψ（yaw）：\n",
    "    机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。\n",
    "![yaw](http://www.123kuai.com/uploadfile/2014/0114/0_1303867644Dzds.gif)\n",
    "\n",
    "   这里面，每一个姿态角都是在描述，在一次旋转变换中，坐标系以自身某一坐标轴为周线旋转相应的角度。因此，当发生一次旋转后，之后的旋转变化都是相对于当下的坐标系而言，与初始坐标系无关，因此旋转变换与旋转轴选取顺序有关，也进而可知任意两互为可相互旋转的坐标系，可通过多种旋转变换转换。\n",
    "\n",
    "   为了使分析统一化，我们采用如下图所示的形式来解释欧拉角：\n",
    "   ![Eulerian angle](http://images.cnitblog.com/blog/394589/201307/10221945-bf5af8ec672247bbba9cde3bcd5c7afa.png)\n",
    "   如图所示：我们采取欧拉角变换的步骤为：\n",
    "   - 首先，选取变换前后两坐标系各自的OXY平面的交线，记作N\n",
    "   - 其次，我们以z轴为轴线，旋转坐标系使得x轴与N重合\n",
    "   - 再者，我们以N为轴线（也就是以当下的x轴为轴线），旋转坐标系是的z轴与目标坐标系的z轴重合\n",
    "   - 最后，我们以z轴为轴线（此时z轴已经是目标坐标系下的z轴了），旋转坐标系使得x轴与目标坐标系的x轴重合。\n",
    "   \n",
    ">   其实，我并不觉得这样的欧拉角描述合适，我们不妨换一种形式思考（其实还有很多种，可以思考）：  \n",
    ">   我们假设坐标系$S_2$是由坐标系$S_1$经过x、y、z逐次旋转相应的角度得到，那么我们自然可以逆向逐次还原：\n",
    ">   - 首先，我们取坐标系$S_2$的平面OXY与坐标系$S_1$的平面OYZ的交线，记作N\n",
    ">   - 其次，我们得到坐标系$S_2$的y轴与N线的夹角，记作$\\psi$，并转动z轴$\\psi$角度，使得y轴与N轴重合，此时y轴便回归原位\n",
    ">   - 然后，我们取此时的坐标系$S_2$的x轴与$S_1$坐标系的x轴的夹角，记作$\\theta$，并转动y轴，使得$S_2$坐标系的x轴与$S_1$坐标系的x轴重合\n",
    ">   - 最后，我们取此时的坐标系$S_2$的y轴与$S_1$坐标系的y轴的夹角，记作$\\phi$，并转动x轴，使得$S_2$坐标系的z轴与$S_1$坐标系的z轴重合\n",
    ">   \n",
    ">   此时，三个角度$\\phi、\\theta、\\psi$分别就是roll、pitch、yaw。\n",
    "   "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 二、四元数法\n",
    "\n",
    "**什么是四元数？**  \n",
    "    四元数就是由四种不同维度的量来描述一个状态量，这种状态量可以描述一种变化，也可以描述一个位置。  \n",
    "    - 当四元数的模为1时，也即是单位四元数，可以表达一种旋转变化；\n",
    "    - 当四元数模不为1时，四元数可分为模×单位四元数，可以表达一种伸长且旋转的变化；\n",
    "    - 当四元数为纯四元数时，也即是纯量为0时，可以描述三维空间中的一个笛卡尔坐标点。\n",
    "    \n",
    "    四元数基本形态为：q=a+bi+cj+dk 其中，a、b、c、d为实数，i、j、k为虚数表示三个正交方向\n",
    "    \n",
    "**四元数的一些性质**  \n",
    "    参见[Wikipedia](https://www.wikiwand.com/zh/%E5%9B%9B%E5%85%83%E6%95%B8#)  \n",
    "    要注意，数乘部分写错了，看的时候要多小心。\n",
    "    \n",
    "**主要思想**  \n",
    "    四元数法，主要是依靠不断的更新四元数，从而使四元数构建的坐标系，更加逼近于当下实际机体坐标系，从而得知Roll、Pitch、Yaw的角度，**注意，这里可以求得三个角度，其他方法一般只可以求得两个，第三个还需要另加磁力计来融合求解**。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 三、一阶互补法\n",
    "  \n",
    "可以说这个方法类似于[GHKFilter滤波器](http://nbviewer.jupyter.org/github/w407022008/All-of-Notes/blob/master/Kalman-Bayesian-Filter-Notes/01%20GHK-filter--Notes.ipynb)中的GFilter，其基本思想就是，既然加速度计和陀螺仪都可以得到Pitch、Roll角，那不妨将其加权融合。\n",
    "    \n",
    "    具体操作呢，主要是由于不能传感器的测量准确度不同。  \n",
    "    三轴加速度在计算角度过程中不存在积累误差，可以直接通过atan()求出，但它包含了太多的噪声，比如机体在做加速运动时引入的加速度、电机运行时产生的震动等等。\n",
    "    陀螺仪呢，它可以直接获得机体三轴的角加速度，而且不易受到外界的干扰，所以精度较高，不过我们需要对他进行离散求和求积分，容易引入离散误差。\n",
    "    因此，综上，我们可以全用一个定值作为权重附加给两个传感器的输出值上，一般情况下，显然在动态机体中，陀螺仪精度要远比三轴加速度计高精度。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 四、[卡尔曼滤波](https://github.com/w407022008/All-of-Notes/tree/master/Kalman-Bayesian-Filter-Notes)\n",
    "\n",
    "    卡尔曼滤波法和一阶互补法类似，原因在卡尔曼滤波笔记中有讲到。其主要思想是，将陀螺仪输出值作为一个状态量，加速度计输出值作为测量量，从而优化Pitch、Roll的估计值。在使用过程中，可以对测量值也就是加速度计输出值先在短时进行平滑处理，然后再向前选取样本，求得短时内测量的方差。但在动态情况下，可以再乘一个系数使系统更相信陀螺仪。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "[下载算法](https://drive.google.com/open?id=0Bzmx-vgdNPujbnRLOEphSEN5Nlk)\n",
    "``` c\n",
    "//1:一阶卡尔曼双测量滤波器  \n",
    "      float angle_m_x=angleAx,gyro_m_x=gx;\n",
    "      angle_x = angle_x + angle_x_dot*dt;\n",
    "    \n",
    "      P_x[0][0] += (P_x[1][0]+P_x[0][1])*dt + P_x[1][1]*dt*dt + Q_angle;\n",
    "      P_x[0][1] += P_x[1][1]*dt;\n",
    "      P_x[1][0] += P_x[0][1];\n",
    "      P_x[1][1] += Q_velo;\n",
    "    \n",
    "      E = (P_x[0][0]+R_angle)*(P_x[1][1]+R_velo)-P_x[0][1]*P_x[1][0];\n",
    "      K[0][0]= ((P_x[1][1]+R_velo)*P_x[0][0]-P_x[0][1]*P_x[1][0])/E;\n",
    "      K[0][1]= ((P_x[0][0]+R_angle)*P_x[0][1]-P_x[0][0]*P_x[0][1])/E;\n",
    "      K[1][0]= (P_x[1][0]*(P_x[1][1]+R_velo)-P_x[1][1]*P_x[1][0])/E;\n",
    "      K[1][1]= (P_x[1][1]*(P_x[0][0]+R_angle)-P_x[1][0]*P_x[0][1])/E;\n",
    "    \n",
    "      P_x[0][0] += -K[0][0]*P_x[0][0] - K[0][1]*P_x[1][0];\n",
    "      P_x[0][1] += -K[0][0]*P_x[0][1] - K[0][1]*P_x[1][1];\n",
    "      P_x[1][0] += -K[1][1]*P_x[1][0] - K[1][0]*P_x[0][0];\n",
    "      P_x[1][1] += -K[1][0]*P_x[0][1] - K[1][1]*P_x[1][1];\n",
    "    \n",
    "      angle_x += K[0][0]*(angle_m_x-angle_x) + K[0][1]*(gyro_m_x-angle_x_dot);\n",
    "      angle_x_dot += K[1][0]*(angle_m_x-angle_x) + K[1][1]*(gyro_m_x-angle_x_dot);\n",
    "```\n",
    "``` c\n",
    "//0:二阶卡尔曼双参滤波器\n",
    "      float angle_m_x=angleAx,gyro_m_x=gx;\n",
    "      angle_x = angle_x + angle_x_dot*dt + 1/2*angle_x_dot2*dt*dt;\n",
    "      angle_x_dot = angle_x_dot + angle_x_dot2*dt;\n",
    "      angle_x_dot2 = angle_x_dot2;\n",
    "    \n",
    "      P_x[0][0] += (P_x[0][1]+P_x[1][0])*dt + (1/2*P_x[2][0]+P_x[1][1]+1/2*P_x[0][2])*dt*dt + (1/2*P_x[2][1]+1/2*P_x[1][2])*dt*dt*dt + 1/4*P_x[2][2]*dt*dt*dt*dt + Q_angle;\n",
    "      P_x[0][1] = (P_x[0][1]+P_x[1][1]*dt+1/2*P_x[2][1]*dt*dt) + (P_x[0][2]+P_x[1][2]*dt+1/2*P_x[2][2]*dt*dt)*dt;\n",
    "      P_x[0][2] = (P_x[0][2]+P_x[1][2]*dt+1/2*P_x[2][2]*dt*dt);\n",
    "      P_x[1][0] += (P_x[2][0]+P_x[1][1])*dt + (P_x[2][1]+1/2*P_x[1][2])*dt*dt + 1/2*P_x[2][2]*dt*dt*dt;\n",
    "      P_x[1][1] = P_x[1][1]+P_x[2][1]*dt + (P_x[1][2]+P_x[2][2]*dt)*dt + Q_velo;\n",
    "      P_x[1][2] = P_x[2][2]*dt;\n",
    "      P_x[2][0] += P_x[2][1]*dt + 1/2*P_x[2][2]*dt*dt;\n",
    "      P_x[2][1] += P_x[2][2]*dt;\n",
    "      P_x[2][2] += Q_acce;\n",
    "    \n",
    "      error_angle = angle_m_x - angle_x;\n",
    "      error_velo = gyro_m_x-angle_x_dot;\n",
    "      \n",
    "      E = (P_x[0][0]+R_angle)*(P_x[1][1]+R_velo)-P_x[0][1]*P_x[1][0];\n",
    "      K[0][0]= ((P_x[1][1]+R_velo)*P_x[0][0]-P_x[0][1]*P_x[1][0])/E;\n",
    "      K[0][1]= ((P_x[0][0]+R_angle)*P_x[0][1]-P_x[0][0]*P_x[0][1])/E;\n",
    "      K[1][0]= (P_x[1][0]*(P_x[1][1]+R_velo)-P_x[1][1]*P_x[1][0])/E;\n",
    "      K[1][1]= (P_x[1][1]*(P_x[0][0]+R_angle)-P_x[1][0]*P_x[0][1])/E;\n",
    "      K[2][0]= (P_x[2][0]*(P_x[1][1]+R_velo)-P_x[2][1]*P_x[1][0])/E;\n",
    "      K[2][1]= (P_x[2][1]*(P_x[0][0]+R_angle)-P_x[2][0]*P_x[0][1])/E;\n",
    "      \n",
    "      P_x[2][0] -= K[2][0]*P_x[0][0]+K[2][1]*P_x[1][0];\n",
    "      P_x[2][1] -= K[2][0]*P_x[0][1]+K[2][1]*P_x[1][1];\n",
    "      P_x[2][2] -= K[2][0]*P_x[0][2]+K[2][1]*P_x[1][2];\n",
    "      static float p0=P_x[0][0],p1=P_x[0][1],p2=P_x[0][2];\n",
    "      P_x[0][0] -= K[0][0]*P_x[0][0]+K[0][1]*P_x[1][0];\n",
    "      P_x[0][1] -= K[0][0]*P_x[0][1]+K[0][1]*P_x[1][1];\n",
    "      P_x[0][2] -= K[0][0]*P_x[0][2]+K[0][1]*P_x[1][2];\n",
    "      P_x[1][0] -= K[1][0]*p0+K[1][1]*P_x[1][0];\n",
    "      P_x[1][1] -= K[1][0]*p1+K[1][1]*P_x[1][1];\n",
    "      P_x[1][2] -= K[1][0]*p2+K[1][1]*P_x[1][2];\n",
    "    \n",
    "      angle_x += K[0][0]*error_angle+K[0][1]*error_velo;\n",
    "      angle_x_dot += K[1][0]*error_angle+K[1][1]*error_velo;\n",
    "      angle_x_dot2 += K[2][0]*error_angle+K[2][1]*error_velo;\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "如果不想深入研究KalmanFilter算法的，尤其不对非线性KalmanFilter感兴趣的，可以直接调用[库函数程序](http://www.arduinolibraries.info/libraries/kalman-filter-library)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 五、[四元数法](http://nbviewer.jupyter.org/github/w407022008/All-of-Notes/blob/master/some-learning-notes-for-chips%26sensors/IMU/MPU6050/%E5%9B%9B%E5%85%83%E6%95%B0%E4%BD%8D%E5%A7%BF%E8%A7%A3%E7%AE%97.ipynb)\n",
    "\n",
    "    四元数法是在做三维空间内旋转变换的最有效的解法，可以有效地避免欧拉角具有的万向节互锁硬确定，同时又是支持三维量同时求解的方法，因此，在一般对三个方向转动都有需求，且都是大幅度转角情况下，比如说飞行器翻筋斗动作时，我们就要采用四元数法来代替卡尔曼求解欧拉角的方法。这里代替的是欧拉角的方法，卡尔曼是滤波器，通常对四元数法精度要求较高时，也需要引入卡尔曼滤波器来弱化噪音。\n",
    "    \n",
    "这里给出[dmp库函数](https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050),有兴趣的可以研究一下四元数相关内容，我这里也有一些[总结笔记](http://nbviewer.jupyter.org/github/w407022008/All-of-Notes/blob/master/%E4%B8%B2%E8%81%94%E6%9C%BA%E5%99%A8%E4%BA%BA/%E5%AF%B9%E5%9B%9B%E5%85%83%E6%95%B0%E7%9A%84%E8%BF%9B%E9%98%B6%E5%AD%A6%E4%B9%A0.ipynb)。\n",
    "\n",
    "[dmp库使用方法](https://create.arduino.cc/projecthub/Aritro/getting-started-with-imu-6-dof-motion-sensor-96e066)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}