技术文档GPS-aided INS Solution for OpenPilot的理解

GPS-aided INS Solution for  OpenPilot       

作者:Dale E. Schinstock

            来自:堪萨斯州立大学（Kansas State University简称KSU或者K-State，成立于1863年2月，是一所世界知名的百年名校)

            openpilot 是一个开源的自动驾驶(驾驶代理),它实行 Hondas 和 Acuras 的自适应巡航控制(ACC)和车道保持辅助系统(LKAS)的功能；

原文下载地址：https://download.csdn.net/download/xiaoxilang/10412589

1.系统模型介绍===============================================================

这里开发的动态系统模型是一个六自由度刚体的运动学模型，其位置和速度用惯性坐标系（地球固定）表示，角速度和加速度在体内固定框架中表示。 因为它是一种运动学模型，它适用于任何车辆，与车辆的具体动力无关。 

除了动态状态变量之外，状态向量还包括传感器的偏置状态，这些状态被动态建模为简单的随机变量;

系统模型的开发将使用两个矩阵，这些矩阵在此不加推导。 这些矩阵中的

第一个矩阵是作为单位四元数的函数的旋转矩阵。

  旋转矩其中的参数为   

值得注意的是，旋转矩阵的倒数是它的转置:

这些矩阵中的第二个矩阵用于“捷联”方程中，用于将机体轴角速度转化为单位四元数：

第二个矩阵：

    

参数命名说明：

，，，	非线性状态方程，非线性测量方程，状态向量式，地球重力加速度
F，G，H，	线性化系统矩阵,线性化状态矩阵,线性化测量矩阵,测量噪声向量
，	NED坐标下的位置向量，NED下的速度向量
，	姿态四元素，陀螺仪角速度偏差
	机体坐标的真实加速度计和旋转速度
，，，	过程噪声向量，加计的噪声向量，陀螺加速度的噪声向量，随即模型的噪声向量
，	机体的加速度计预测值,输入向量
，	机体角速率转换为四元数矩阵；机体坐标到地理坐标的旋转矩阵
，，	气压高度测量，地理坐标的磁力计，机体坐标的磁力计
，，	测量值向量，来自状态矢量的预测结果，矢量的离散时间版本
，，	状态转换矩阵，输入矩阵的采样时间，预测周期
	传递噪声的协方差矩阵
	协方差矩阵的预测噪声
，	卡尔曼增益矩阵，状态预测协方差

2 系统模型的成员 ==========================================================

状态变量

由INS / GPS系统估计的状态变量是位置，速度，姿态（单位四元数）和陀螺仪速率偏差。如（3）

输入（输入加计和陀螺）

动态系统的真实输入即运动系统是真实的角速度和加速度向量。 但是，我们将INS建模为动态系统，该系统使用包含噪声，偏差和重力的这些向量的测量值。

此处，加速度计测量包括用Rbe旋转进入车身固定框架的地球重力加速度。 此外，在不失一般性的情况下，传感器噪声在此显示为减法而不是加法。 这完全是为了方便在下面开发的状态方程中完成的，其中它将成为累加的。

状态方程

状态方程是状态变量的导数。 一般来说，这些方程是状态变量和输入的非线性函数

（5）中的前三个向量方程是六自由度刚体的运动学方程。 对于偏置状态，最后的矢量方程是随机游走，用于以简单的方式对随机变化缓慢的状态动态进行建模。虽然（5）捕捉刚体的运动学，但它并不适用于状态方程。 状态方程应该写成状态，输入和过程/干扰噪声的函数：；这可以通过求解（4）真实的角速度和加速度来完成。

然后将它们插入（5），得到状态方程作为状态的函数；如（6）

其中输入为：  

过程噪声为：

输出

动态系统模型的输出是由传感器测量的用于校正步骤的变量。 它们必须作为状态变量的函数形成:

此处，机体框架中的磁场表示为地球框架中的恒定磁场和作为四元数的函数的旋转矩阵的函数。 气压高度计输出仅是位置矢量下方分量的负值。 存在来自与传感器噪声相加的输出矢量对应的传感器的测量矢量：

这两个向量之间的差值z-y用于校正步骤的反馈，以通过数值积分校正预测步骤中预测的状态

线性化

  为了实现EKF，需要在预测步骤的每次计算中线性化状态方程，并且在每次计算所述校正步骤时对输出方程进行线性化，以便可以使用KF方程。 该线性化导致以下雅可比矩阵的方程式，

计算F中元素的偏导数给出，其中

其中：,

计算G中元素的偏导数给出，其中

计算H中元素的偏导数给出，其中

其中,,

3,扩展卡尔曼滤波======================================================

扩展的卡尔曼过滤器实施

对EKF和KF的一般描述超出了本讨论的范围。 然而，关于这些具体实现的一些细节在将算法与在许多来源中找到的更一般的EKF方程联系起来可能是重要的。 KF算法最终是一个离散时间算法。 它的根源在于它基于一个非常简单的系统离散模型。

 离散时间干扰/过程噪声矢量被假定为白色并且具有噪声协方差矩阵Q. 测量噪声被假定为白色并且具有噪声协方差矩阵R。 这些都是离散时间噪声过程的事实并不存在问题，因为传感器最终在离散时间被采样。 事实上，这是一个好处，因为可以直接估计数据样本中的噪声方差。 所实现的计算机算法确实做出这些矩阵是对角线的简化假设，即每个传感器上的噪声是独立的。我们的模型是连续时间模型。 因此，我们必须使用一些离散时间的近似值来实现。 我们使用以下一阶近似方程如下：

INS对输入进行数值积分，即加速度和角速度测量，以获得位置，速度和方向的估计值。 

EKF的预测步骤使用INS的输出，并且还预测状态估计误差的协方差的增长。 这个协方差是对我们对估计状态信心的一个近似估计。 这个协变的大小，并希望是真实的

  在我们将其他传感器并入反馈回路的校正步骤中，状态估计中的误差减少。

4 INS / GPS算法(下划红线的分别是ekf的五个黄金公式的说明参加我的博客文章)
预测步骤

更正/更新步骤

步骤（p1）在RungeKutta（X，U，dT）中完成。它通过对实现（6）的StateEq（X，U，Xdot）进行函数调用，实现与四阶Runga Kutta算法的数值积分。
步骤（p3）在LinearizeFG（X，U，F，G）中完成。它实现了（8）和（9）
步骤（p4）在协方差预测（F，G，Q，dT，P）中完成。它估计由于过程噪声导致的状态估计误差的协方差的增长。
步骤（c1）在LinearizeH（X，Be，H）中完成。它实现（10）。
步骤（c2）在MeasurementEq（X，Be，Y）中完成。它执行（7）。

步骤（c3）在SerialUpdate（H，R，Z，Y，P，X，SensorsUsed）中完成。虽然它实现了与此步骤中所示等式相同的效果，但它采用了非常不同的算法。这些方程通过串行更新算法实现，分别处理每个标量测量[1，通道4.2] [2，通道4.5]。这避免了通过用标量分割来替代矩阵求逆。它计算效率高，数值稳定。此外，它允许在单个校正步骤中使用任何选定的传感器子集。使用这种串行更新算法是可能的，因为假定每个测量的噪声是不相关的，即协方差矩阵R是对角的。

参考文献

[1] Grewal, M.S., A.P. Andrews, Kalman Filtering, Theory and Practice Using MATLAB, John Wiley  and Sons, Inc.,2001

[2] Farrell, J.A., M. Barth, The GlobalPositioning  System & Inertial Navigation, McGraw Hill, 1999