q = a b i c j d k ( a , b , c , d ∈ R ) q = a bi cj dk \qquad(a, b, c, d \in R) q=a bi cj dk(a,b,c,d∈R) 其中： i 2 = j 2 = k 2 = i j k = ? 1 i^{2} = j^{2} = k^{2} = ijk = -1 i2=j2=k2=ijk=−1 与复数类似，因为四元数其实就是对于基 1 , i , j , k {1, i, j, k} 1,i,j,k的线性组合，四元数写成向量的形式： q = [ a b c d ] q = \begin{bmatrix} a\\ b\\ c\\ d\\ \end{bmatrix} q=⎣⎢⎢⎡​abcd​⎦⎥⎥⎤​ 除此之外，我们经常将四元数的实部与虚部分开，并用一个三维的向量来表示虚部，将它表示为标量和向量的有序对形式: q = [ s v ] ( V = [ x y z ] s , x , y , z ∈ R ) q = \begin{bmatrix} s & \bf{v} \end{bmatrix} (V = \begin{bmatrix} x \\ y \\ z\\ \end{bmatrix} \quad s,x, y, z \in R ) q=[s​v​](V=⎣⎡​xyz​⎦⎤​s,x,y,z∈R)

惯性导航系统中，加速度计和陀螺仪所在的坐标系称为载体坐标系。在载体运动过程中，载体坐标系在不断变化，而定位结果需要在统一的坐标系下表示，通常定义一个静止的惯性坐标系，称为大地坐标系。 四元数从载体坐标系到大地坐标系的转换矩阵

INS:Inertial Navigation System，惯性导航系统。 GPS:Global Positioning System，全球定位系统。

多传感器信息的综合，通过整合形成对环境某特征的一种表达方式的技术称为信息融合技术。

• 加权平均

  加权平均是将低传感器数据与多个传感器相结合的方法，它具有直观简单的特。

• 贝叶斯估计

  在静态环境中可以使用贝叶斯估计进行融合多传感器的数据。在这种方法中概率分布就是所谓的信息，在加高斯噪声的不确定性的情况一般使用这种算法。

• 卡尔曼滤波

  KF滤波是根据测量模型的统计特性进行递推的，常使用它处理实时融合动态的低层次冗余传感器数据。

位置、速度和姿态角，系统的姿态如下：

陀螺仪的角速度反映了载体的姿态变化，角速度用 ω i b b \omega^{b}_{ib} ωibb​表示:

假设载体坐标系下，三轴加速度值为 f b f^{b} fb: 在大地坐标系下，三轴加速度应为： 式中， g i g^{i} gi为当前大地坐标系下的重力向量， C b i C^{i}_{b} Cbi​为当前坐标转换矩阵。

位置、速度和姿态的INS/GPS传播模型 离散化

卡尔曼滤波器是一种基础预测定位算法， 是一种利用线性系统状态方程 ，含义是现时刻的最佳估计为在前一时刻的最佳估计的基础上根据现时刻的观测值作线性修正。本质上就是预测（Predict，或者叫传播Propagation）到更新（update）两个过程的迭代。 预测： 更新： 公式中的符号说明： θ k \theta_k θk​真实值，A状态转移矩阵， B , u k B ,u_{k} B,uk​系统模型参数，C观测矩阵， S k S_{k} Sk​过程噪声，Q协方差矩阵， < θ k > <\theta_k> <θk​>估计值， z k z_k zk​观测值， θ k ′ \theta^{'}_{k} θk′​预测值,估计值与真实值之间的误差协方差矩阵 ∑ k \sum_k ∑k​,。 算法流程如下：

1. 数据准备。 根据公式（1）和公式（2）计算预测值 θ k ′ \theta^{'}_{k} θk′​以及预测值和真实值之间的协方差矩阵 ∑ k ′ \sum^{'}_{k} ∑k′​;
2. 滤波估计。根据公式（3）和公式（4）计算卡尔曼增益 K k ′ K^{'}_{k} Kk′​，然后根据公式（5）估计此时猪头的重量 < θ k > <\theta_{k}> <θk​>；
3. 参数更新。根据公式（6）计算估计值与真实值之间的误差协方差矩阵 ∑ k ′ \sum^{'}_{k} ∑k′​，用于下一次递推。

EKF： Extended Kalman filter 扩展卡尔曼滤波是基于卡尔曼滤波对非线性系统进行一阶泰勒展开。在当前状态最优估计点线性化，分析当前误差状态线性化的传播过程。扩展卡尔曼滤波的难点在于，非线性系统的函数的雅克比矩阵较为复杂，而雅克比矩阵的准确性是影响滤波性能的关键因素之一。

扩展卡尔曼滤波算法将非线性方程进行泰勒级数展开，去掉二阶以上的高阶项，使非线性方程线性化。

扩展卡尔曼滤波EKF的状态转移方程和观测方程为 利用泰勒展开式对（1）式在上一次的估计值 < θ k − 1 > <\theta _{k-1}> <θk−1​>处展开得 再利用泰勒展开式对(2）式在本轮的状态预测值 θ \theta θ处展开得 其中， F k − 1 \bf{F}_{k - 1} Fk−1​和 H k \bm{H}_{k} Hk​分别表示函数 f ( θ ) f(\theta) f(θ)和 h ( θ ) h(\theta) h(θ)在 < θ k − 1 > <\theta_{k - 1}> <θk−1​>和 θ k ′ \theta^{'}_{k} θk′​处的雅克比矩阵。 EKF的预测与更新： 预测： 更新： 其中的雅克比矩阵 F k − 1 \bf{F}_{k - 1} Fk−1​和 H k \bm{H}_{k} Hk​分别为

UKF： Unscented Kalman filter UKF=UT+KF 无迹卡尔曼滤波的主要思想是，在状态点周围利用高斯随机分布设置多个采样点，这些采样点被称为Sigma点。直接利用非线性函数进行计算，并根据权重共同构成下一个状态点。在无迹卡尔曼滤波算法中，状态分布采用高斯随机变量表示，通过采用一些确定的Sigma点来描述高斯随机变量的特征。Sigma点集合具有和高斯随机变量相同的均值和方差．这些采样点通过非线性函数传播后，后验均值和方差可以达到⒉阶精度，而扩展卡尔曼滤波算法只能达到1阶精度。

UT变换是用固定数量的参数去近似一个高斯分布，其实现原理为：在原先分布中按某一规则取一些点，使这些点的均值为协方差状态分布与原状态分布的均值和协方差相等；将这些点代入非线性函数中，相应得到非线性函数值点集，通过这些点集可求取变换的均值和协方差。UT变换的实现一般分为以下三个步骤： （1）构造Sigma点集 根据随机变量 x \mathbf x x的统计量均值 x ˉ \bar{\mathbf x} xˉ和协方差 P x P_{x} Px​，构造Sigma点集 其中 λ \lambda λ为尺度参数，调整其值可以提高逼近精度， n n n为变量 x \mathbf x x的维度。用这一组采样点可以近似地表示变量 x \mathbf x x的高斯分布。