{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 四元数的位姿解算"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "在给出位姿解算的方法之前，我们先来看看四元数所表达的意义，和正交矩阵的关系。\n",
    "\n",
    "我们设有：  \n",
    "  \n",
    "单位四元数：$q=cos\\frac{\\theta}{2}+(ai+bj+ck)sin\\frac{\\theta}{2}=q_0+q_1i+q_2j+q_3k$   其中，$a^2+b^2+c^2=1$  \n",
    "纯四元数：$p=xi+yj+zk$  \n",
    "    \n",
    "则，经过旋转变化可以得到一个新的纯四元数。这里我们需要知道，**四元数的数乘运算表达旋转的过程**，具体可以参照四元数的数学理论。\n",
    "\n",
    "那么，有：$p_2=qp_1q^{-1}$   这里，$q^{-1}$是四元数q的逆，就是$q^{-1}=q_0-q_1i-q_2j-q_3k$，当$|q|=1$时，也有$q^{-1}=q^*$也就是共轭。\n",
    "\n",
    "于是，有：$$\\left\\lgroup \\matrix{x_2 \\cr y_2 \\cr z_2} \\right\\rgroup = M(q) \\left\\lgroup \\matrix{x_1 \\cr y_1 \\cr z_1} \\right\\rgroup=\\left\\lgroup \\matrix{q_0^2+q_1^2-q_2^2-q_3^2&2(q_1q_2-q_0q_3)&2(q_1q_3+q_0q_2） \\cr 2(q_1q_2+q_0q_3)&q_0^2-q_1^2+q_2^2-q_3^2&2（q_2q_3-q_0q_1) \\cr 2(q_1q_3-q_0q_2)&2(q_2q_3+q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2} \\right\\rgroup \\left\\lgroup \\matrix{x_1 \\cr y_1 \\cr z_1} \\right\\rgroup$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "同样，也可以有：$p_0=q^{-1}p_1q$表示为：  \n",
    "\n",
    "$$\\left\\lgroup \\matrix{x_0 \\cr y_0 \\cr z_0} \\right\\rgroup = M(q)^T \\left\\lgroup \\matrix{x_1 \\cr y_1 \\cr z_1} \\right\\rgroup=\\left\\lgroup \\matrix{q_0^2+q_1^2-q_2^2-q_3^2&2(q_1q_2+q_0q_3)&2(q_1q_3-q_0q_2) \\cr 2(q_1q_2-q_0q_3)&q_0^2-q_1^2+q_2^2-q_3^2&2(q_2q_3+q_0q_1) \\cr 2(q_1q_3+q_0q_2）&2（q_2q_3-q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2} \\right\\rgroup \\left\\lgroup \\matrix{x_1 \\cr y_1 \\cr z_1} \\right\\rgroup$$\n",
    "\n",
    "这里的$p_0、p_1、p_2$表示$p_0、p_1$分别进行q变换得到$p_1、p_2$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**与变换矩阵的关系**  \n",
    "\n",
    "我们将单位四元数的表达式带入$M(q)$中，可以得到：$$M(q)=\\left\\lgroup \\matrix{cos\\theta+a^2(1-cos\\theta) & ab(1-cos\\theta)-csin\\theta & bsin\\theta + ac(1-cos\\theta) \\cr ab(1-cos\\theta)+csin\\theta & cos\\theta+b^2(1-cos\\theta) & -asin\\theta+bc(1-cos\\theta) \\cr -bsin\\theta+ac(1-cos\\theta) & asin\\theta+bc(1-cos\\theta) & cos\\theta+c^2(1-cos\\theta)} \\right\\rgroup$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**单位四元数的变换等效于按顺序绕x、y、z轴转动$X、Y、Z$角度所产生的变换矩阵**  \n",
    "也就是说：  \n",
    "\n",
    "$$M(q)=R_zR_yR_x=\\left\\lgroup \\matrix{cosY cosY & sinX sinYcosZ -cosX sinZ & cosXsinYcosZ+sinxsinZ \\cr cosYsinZ & sinXsinYsinZ+cosXcosZ & cosXsinYsinZ-sinXcosZ \\cr -sinY & sinXcosY & cosXcosY} \\right\\rgroup$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "因此，才有了反求出的欧拉角：  \n",
    "$$Roll=X=atan\\frac{m_{21}}{m_{22}}$$\n",
    "$$Pitch=Y=-asin(m_{20})$$\n",
    "$$Yaw=Z=atan\\frac{m_{10}}{m_{00}}$$  \n",
    "$m_{ij}$表示矩阵当中第i行第j列的元素,,当然这里的RollPitchYaw是对于特定坐标系的，详细查看[这里](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)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 算法\n",
    "\n",
    "接下来我们给出四元数法的基本算法："
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "初始化：  \n",
    ">$K_p = 100;$#PI控制的P调节  \n",
    "$K_i = 0.02;$#PI控制的I调节  \n",
    "$T = 0.001;$#采样周期  \n",
    "$q_0=1;q_1=q_2=q_3=0;$#初始四元数假设  \n",
    "\n",
    "循环主体：  \n",
    "①**传感器测量值**，在机体坐标系下，测量到世界坐标系下绝对矢量——重力加速度g的姿态方向矢量。  \n",
    ">$norm=sqrt(a_x^2+a_y^2+a_z^2);$#计算模  \n",
    "$a_x /= norm$  \n",
    "$a_y /= norm$  \n",
    "$a_z /= norm$#归一化，计算出当前机体坐标系下重力的方向  \n",
    "\n",
    "②**状态估计值**，在机体坐标系下，通过$Q\\rightarrow M$关系结算的世界坐标系下绝对矢量——重力加速度g的姿态方向矢量。  \n",
    ">$V_x=2(q_1q_3-q_0q_2)$  \n",
    "$V_y=2(q_0q_1+q_2q_3)$  \n",
    "$V_z=q_0^2-q_1^2-q_2^2+q_3^2$ \\\\计算重力在当前假设坐标系下的方向，其实就是$M(q)$矩阵当中的最后一列，也就是它与$\\left\\lgroup \\matrix{0 \\cr 0 \\cr 1} \\right\\rgroup$相乘  \n",
    "\n",
    "③获取同一坐标系下的测量值与估计值误差。      \n",
    ">$error = a \\times V$  \\\\获得两坐标系对待同一重力的方向描述的误差  \n",
    "\n",
    "④通过PI互补滤波器，将姿态误差补偿到角速度上，修正角速度积分漂移。  \n",
    ">$exInt = error_x·K_i$ \\\\ 积分误差反馈  \n",
    "$eyInt = error_y·K_i$  \n",
    "$ezInt = error_z·K_i$  \n",
    "\n",
    ">$g_x += K_p·error_x + exInt$  \n",
    "$g_y += K_p·error_y + eyInt$  \n",
    "$g_z += K_p·error_z + ezInt$#为保持假设坐标系能够随机体转动而转动，同时接受误差的作用，从而获得当前假设坐标系应有的转动变换  \n",
    "\n",
    "⑥通过一届龙格库塔（欧拉求积）求解四元数微分方程，即四元数方向空间经过修正角速度作用发生旋转过程的积分，直接数乘以g相当于对t求过导数了，具体的看[这里](http://blog.csdn.net/qq_29413829/article/details/60973915),这个博客写的挺细的了。     \n",
    ">$q_0 += \\frac{T}{2}(-q_1g_x-q_2g_y-q_3g_z)$  \n",
    "$q_1 += \\frac{T}{2}(q_0g_x+q_2g_z-q_3g_y)$  \n",
    "$q_2 += \\frac{T}{2}(q_0g_y-q_1g_z+q_3g_x)$  \n",
    "$q_3 += \\frac{T}{2}(q_0g_z+q_1g_y-q_2g_x)$#四元数更新，也是更新当前假设坐标系  \n",
    "\n",
    "⑦ \n",
    ">归一化四元数  \n",
    "\n",
    "⑧位姿解算，获得机体坐标系顺次绕轴x、y、z角度Roll、Pitch、Yaw后得到世界坐标系过程的欧拉角：  $57.3=\\frac{180}{\\pi}$   \n",
    ">$Yaw=atan\\frac{2q_2q_2-2q_0q_3}{2q_0^2+2q_1^2-1}\\times57.3$  \n",
    "$Roll=atan\\frac{2q_1q_0+2q_2q_3}{2q_0^2+2q_3^2-1}\\times57.3$  \n",
    "$Roll=-asin(2q_1q_3-2q_0q_2)\\times57.3$  "
   ]
  }
 ],
 "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
}