一种基于递归最小二乘的车辆侧倾角与俯仰角估计方法

本发明涉及一种基于递归最小二乘的车辆侧倾角与俯仰角估计方法，其目的在于对 汽车行驶的动力学过程进行适当的建模，并利用带遗忘因子的递归最小二乘方法获得车辆 侧倾角与俯仰角估计值，这些估计值可用于车辆组合导航与定位，具有精度高、成本低、 实时性好等显著优点，属于汽车定位导航领域。

近年来，智能交通系统ITS（Intelligent Transportation Systems）在世界范围内得到了 高度的重视和快速的发展，ITS功能有效发挥的一个重要基础是实现车辆的准确可靠定 位。在车辆准确可靠定位的情况下，ITS可以更有效的诱导车辆，提高运行效率，改善行 车安全，减少尾气排放。

目前，车辆导航定位领域应用最多的是GPS技术。但由于遮挡或多路径现象，GPS 会出现定位不准甚至失效的问题，无法实现准确可靠的连续定位。为克服GPS的不足， 车辆多传感器组合导航的研究引起了广泛的重视，即在GPS失效的情况下，利用惯导传 感器或航位推算来进行推算定位，从而在GPS失效时获取较为准确的定位信息。

车辆在实际道路上行驶过程中，由于道路纵向和横向坡度的存在以及车辆悬架的运 动，导致存在着一定的车辆侧倾角和俯仰角，虽然其值一般较小，但其对于推算定位的准 确性所起的作用却难以忽略。在陆地车辆应用中，车辆的加速度往往远小于重力加速度， 以至于较小的侧倾角和俯仰角可能导致在车体坐标系下测量纵向和侧向加速度时产生较 大的误差，这些误差会累积导致推算速度和位置信息时产生较大的累积误差。因此，对侧 倾角和俯仰角等姿态角进行准确的测量或估计是推算出准确的定位信息的重要保证，其准 确性是影响影响车辆组合定位精度的一个重要因素。

通常用来确定侧倾角和俯仰角等姿态角信息的方法是使用完整的六维惯性测量单元 IMU（Inertial Measurement Unit），该IMU包括3个加速度计和3个角速度陀螺仪，车辆 的姿态角信息可以通过六维IMU的捷联算法计算得出。然而，六维IMU价格昂贵，特别 是三个陀螺仪的价格。考虑到许多车辆安装有电子稳定控制或横摆稳定控制系统，部分 IMU信号可以通过车辆的CAN（Controller Area Network，控制器局域网络）总线较容易 的获取，这些信号通常包括横摆角速度、纵向加速度与侧向加速度，为了有效降低成本， 本专利即利用这些可获取的信息来估计车辆的侧倾角和俯仰角。

理论上，如果车辆的初始状态已知且车辆的横摆角速度可获取，车辆的侧倾角和俯仰 角可以通过数值积分方法计算。然而事实上，直接积分方法由于传感器误差和不可避免的 数值运算误差，会导致较大的漂移，特别是在使用车载低成本MEMS （Micro-Electro-Mechanic System，微机电系统）传感器时，因此，本发明并不采用直接 积分方法，而是提出一种实时递归最小二乘（Recursive Least Squares,RLS）算法来对车 辆的侧倾角和俯仰角进行估计。

本发明的目的是克服现有技术的不足，提出一种基于递归最小二乘的车辆侧倾角与俯 仰角估计方法，该方法精度高、成本低、实时性好，能够汽车定位导航的需求。

本发明采用的技术方案如下：一种基于递归最小二乘的车辆侧倾角与俯仰角估计方 法，其特征在于：本发明针对陆地行驶四轮车辆，建立符合其行驶特征的车辆动力学模型， 进一步通过带遗忘因子的递归最小二乘（Recursive Least Squares,RLS）方法实现对车辆 侧倾角与俯仰角的实时、准确估计，且仅需低成本车载传感器，成本较低；具体步骤包括：

1）建立汽车行驶过程的动力学模型

忽略地球旋转速度，假设车辆的俯仰角速度、侧倾角速度与垂向速度为零，则可建立 车辆行驶过程的动力学方程为：

$\begin{array}{c}{\stackrel{.}{v}}_x=a_x+{\omega }_z{v}_y+g\sin \theta \\ {\stackrel{.}{v}}_y=a_y-{\omega }_z{v}_x-g\sin \phi \cos \theta \end{array}---\left(1\right)$

式（1）中，v_x,v_y分别表示车辆的纵向速度和侧向速度，a_x,a_y分别表示车辆的纵向加速度
和侧向加速度，ω_z表示车辆的横摆角速度，上述定义都是针对车体坐标系，g表示重力
加速度，φ,θ分别表示车辆的侧倾角与俯仰角，上标志“·”表示微分，如表示对v_x
的微分；

由（1）式可得

$\begin{array}{c}\theta =\arcsin \left(\frac{{\stackrel{.}{v}}_x-a_x-{\omega }_z{v}_y}{g}\right)\\ \phi =\arcsin \left(\frac{a_y-{\omega }_z{v}_x-{\stackrel{.}{v}}_y}{g\cos \theta }\right)\end{array}---\left(2\right)$

式（2）中，纵向车速的微分可通过纵向车速对时间求导获取，考虑到车辆正常行驶中，v_y
与数值较小因而可以忽略，同时，考虑到在大部分路面情况下，车辆的侧倾角与俯
仰角通常是小角度，即有arcsin(·)≈·，则式（2）可简化为：

$\begin{array}{c}\theta =\arcsin \left(\frac{{\stackrel{.}{v}}_x-a_x}{g}\right)\approx \frac{{\stackrel{.}{v}}_x-a_x}{g}\\ \phi =\arcsin \left(\frac{a_y-{\omega }_z{v}_x-{\stackrel{.}{v}}_y}{g\cos \theta }\right)\approx \frac{a_y-{\omega }_z{v}_x}{g\cos \theta }\end{array}---\left(3\right)$

2）所需车载传感器安装

由式（3）可知，只需测得车辆的纵向速度、纵向加速度、侧向加速度与横摆角速度， 即可利用所建立并合理简化后的车辆行驶动力学方程，即式（3）来估计车辆的俯仰角和 侧倾角；因此，仅需要两个低成本MEMS（Micro-Electro-Mechanic System，微机电系统） 加速度传感器，一个低成本MEMS陀螺仪以及车速传感器即可满足测量要求；

其中两个低成本MEMS加速度传感器安装于车辆质心位置附近，一个沿车体坐标系 纵轴，用以测量纵向加速度，一个沿车体坐标系横轴，用以测量侧向加速度，低成本MEMS 陀螺仪也安装于车辆质心位置附近，沿车体坐标系垂向轴安装，用以测量横摆角速度，车 速传感器用于测量纵向车速，霍尔车速传感器或轮速传感器等传感器均可采用，在此不做 限定，但要求车速测量精度误差小于0.05米/秒，在测得纵向车速信号后，将其对时间求 导即可获取其微分；

3）基于递归最小二乘的车辆侧倾角与俯仰角估计

将式（3）表示为参数识别标准形式：

式（4）中，k表示离散时刻， <math> <mrow> <mi>&gamma;</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mover> <mi>&theta;</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mover> <mi>&phi;</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </math> 表示待估参数矩阵，其中，与分别表示
待估计的车辆俯仰角与侧倾角； <math> <mrow> <mi>y</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mover> <mi>v</mi> <mo>.</mo> </mover> <mrow> <mi>x</mi> <mo>_</mo> <mi>m</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>x</mi> <mo>_</mo> <mi>m</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>a</mi> <mrow> <mi>y</mi> <mo>_</mo> <mi>m</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>&omega;</mi> <mrow> <mi>z</mi> <mo>_</mo> <mi>m</mi> </mrow> </msub> <msub> <mi>v</mi> <mrow> <mi>x</mi> <mo>_</mo> <mi>m</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </math> 表示系统输出矩阵，a_{x_m}、a_{y_m}与
ω_{z_m}分别表示利用低成本MEMS传感器所测得的纵向加速度、侧向加速度与横摆角速度，
v_{x_m}表示通过车速传感器所获取的车辆纵向速度，表示v_{x_m}的微分，由车速传感器所
测得的纵向速度信号对时间求导来获取，即在每一离散时刻k，有：

${\stackrel{.}{v}}_{x\_m}\left(k\right)=\frac{v_{x\_m}\left(k\right)-v_{x\_m}(k-1)}{\mathrm{dt}}---\left(5\right)$

式（5）中，dt表示采样时间间隔，本发明中，dt=0.01（秒）；
表示输入回归矩阵，本发明中上角标T表示对矩阵转置；则利用带遗忘因子的递归最小二
乘（Recursive Least Squares,RLS）算法实时估计车辆侧倾角与横摆角的估计步骤如下：

（1）计算系统输出矩阵y(k)，并计算输入回归矩阵

（2）计算增益矩阵K(k)；其中，方差矩阵
参数λ为遗忘因子，能有效减少不再与模型相关的旧数
据的影响，并防止协方差发散，通常取值范围在[0.9,1]，本发明取0.975；

（3）计算待估参数矩阵γ(k)；

其中I为2×2单位矩阵，至此，可实时估计出车辆侧倾角与横摆角。

本发明的优点及显著效果：

（1）本发明提出了一种低成本、高精度、实时性好的车辆侧倾角和俯仰角估计方法， 可用于车辆组合导航与定位领域对于车辆位置与速度信息进行准确推算需要；

（2）本发明是对车辆动力学模型根据车辆行驶特点进行合理简化，并利用递归最小 二乘算法进行侧倾角和俯仰角的估计，保障了其估计精度与实时性；

（3）本发明仅需量产车上车载传感器，具有成本低的优点，便于大规模推广。

图1是仿真工况1俯仰角估计结果；

图2是仿真工况2侧倾角估计结果；

实施实例1

近年来，智能交通系统ITS（Intelligent Transportation Systems）在世界范围内得到了 高度的重视和快速的发展，ITS功能有效发挥的一个重要基础是实现车辆的准确可靠定 位。在车辆准确可靠定位的情况下，ITS可以更有效的诱导车辆，提高运行效率，改善行 车安全，减少尾气排放。

目前，车辆导航定位领域应用最多的是GPS技术。但由于遮挡或多路径现象，GPS 会出现定位不准甚至失效的问题，无法实现准确可靠的连续定位。为克服GPS的不足， 车辆多传感器组合导航的研究引起了广泛的重视，即在GPS失效的情况下，利用惯导传 感器或航位推算来进行推算定位，从而在GPS失效时获取较为准确的定位信息。

车辆在实际道路上行驶过程中，由于道路纵向和横向坡度以及车辆悬架的运动，导致 存在着一定的车辆侧倾角和俯仰角，虽然其值一般较小，但其对于推算定位的准确性所起 的作用却难以忽略。在陆地车辆行驶过程中，车辆的加速度往往远小于重力加速度，以至 于较小的侧倾角和俯仰角可能导致在车体坐标系下测量纵向和侧向加速度时产生较大的 误差，这些误差会累积导致在推算速度和位置信息时产生较大的累积误差。因此，对侧倾 角和俯仰角等姿态角进行准确的测量或估计是推算出准确的定位信息的重要保证，其准确 性是影响影响车辆组合定位精度的一个重要因素。

通常用来确定侧倾角和俯仰角等姿态角信息的方法是使用完整的六维惯性测量单元 IMU（Inertial Measurement Unit），该IMU包括3个加速度计和3个角速度陀螺仪，利用 IMU输出量和角度信息的微分之间的运动学关系，并忽略地球旋转速度，车辆动力学过 程可建模为[此处可参考文献：H.Eric Tseng a,Li Xu，Davor Hrovat，Estimation of land  vehicle roll and pitch angles[J].Vehicle System Dynamics:International Journal of Vehicle  Mechanics and Mobility,2007,45(5):433-443.]：

$\begin{array}{c}\stackrel{.}{\phi }={\omega }_x+({\omega }_y\sin \phi +{\omega }_z\cos \phi )\tan \theta \\ \stackrel{.}{\theta }={\omega }_y\cos \phi -{\omega }_z\sin \phi \\ \stackrel{.}{\psi }=({\omega }_y\sin \phi +{\omega }_z\cos \phi )/\cos \theta \end{array}---\left(1\right)$

$\begin{array}{c}{\stackrel{.}{v}}_x=a_x+{\omega }_z{v}_y-{\omega }_y{v}_z+g\sin \theta \\ {\stackrel{.}{v}}_y=a_y-{\omega }_z{v}_x+{\omega }_x{v}_z-g\sin \phi \cos \theta ---\left(2\right)\\ {\stackrel{.}{v}}_z=a_z+{\omega }_y{v}_x-{\omega }_x{v}_y-g\cos \phi \cos \theta \end{array}$

式中，ωx,ωy和ωz分别表示围绕车体坐标系纵轴、横轴以及垂向轴的角速度，vx,vy 和vz分别表示沿车体坐标系纵轴、横轴以及垂向轴的线速度，ax,ay和az分别表示沿车 体坐标系纵轴、横轴以及垂向轴的加速度；φ,θ,和ψ分别表示侧倾、俯仰和横摆三个 欧拉角；g表示重力加速度，本发明取9.78。

利用式（1）和（2），车辆的姿态角信息可以通过六维IMU的捷联算法计算得出，大 量车辆定位文献中都有涉及。然而，六维IMU价格昂贵，特别是三个陀螺仪的价格。考 虑到许多车辆安装有电子稳定控制或横摆稳定控制系统，部分IMU信号可以通过车辆的 CAN（Controller Area Network，控制器局域网络）总线较容易的获取，这些信号通常包 括横摆角速度、纵向加速度与侧向加速度；为了有效降低成本，本专利即研究如何利用这 些可获取的信息而非利用完整的6维IMU来估计车辆的侧倾角和俯仰角。

由式（1），可以看出为了估计出侧倾角和俯仰角，并不需要横摆角信息ψ。同时，由 于车辆通常行驶的交通路面的纵向和横向坡度都较小，其横向坡度率与纵向坡度率通常都 小于20%，（本发明即主要针对横向坡度率与纵向坡度率都小于20%的道路），车辆的侧 倾角与俯仰角都是连续缓慢的变化，其相应的角速度值较小可忽略，且车辆垂向速度一般 也较小，因此，可以合理的认为ωx≈0,ωy≈0,νz≈0.则式（1）和（2）可以简化为：

$\begin{array}{c}\stackrel{.}{\phi }={\omega }_z\cos \phi \tan \theta \\ \stackrel{.}{\theta }=-{\omega }_z\sin \phi \end{array}---\left(3\right)$

$\begin{array}{c}{\stackrel{.}{v}}_x=a_x+{\omega }_z{v}_y+g\sin \theta \\ {\stackrel{.}{v}}_y=a_y-{\omega }_z{v}_x-g\sin \phi \cos \theta \end{array}---\left(4\right)$

根据式（3），理论上，如果车辆的初始状态已知且车辆的横摆角速度可获取，车辆的 侧倾角和俯仰角可以通过数值积分方法计算。然而事实上，直接积分方法由于传感器误差 和不可避免的数值运算误差，会导致较大的漂移，特别是使用低成本MEMS传感器时， 因此，本发明并不采用直接积分方法，而是利用式（4），提出一种实时递归最小二乘 （Recursive Least Squares,RLS）算法来对车辆的侧倾角和俯仰角进行估计。

递归最小二乘是对未知矢量的迭代算法，以模型误差的最小方差为目标，对于每个采 样周期，使用已有采样数据通过反复迭代计算未知矢量，具有数据存储量小、算法简便的 特点。

由（4）式可得

$\begin{array}{c}\theta =\arcsin \left(\frac{{\stackrel{.}{v}}_x-a_x-{\omega }_z{v}_y}{g}\right)\\ \phi =\arcsin \left(\frac{a_y-{\omega }_z{v}_x-{\stackrel{.}{v}}_y}{g\cos \theta }\right)\end{array}---\left(5\right)$

式（5）中，纵向车速的微分可通过纵向车速对时间求导获取，考虑到车辆正常行驶中，v_y
与数值较小因而可以忽略，同时，考虑到由于车辆通常行驶的交通路面的纵向和横
向坡度都较小，其横向坡度率与纵向坡度率都小于20%，（本发明即主要针对横向坡度率
与纵向坡度率都小于20%的道路），因此，车辆的侧倾角与俯仰角通常是小角度，即有
arcsin(·)≈·，则式（5）可简化为：

$\begin{array}{c}\theta =\arcsin \left(\frac{{\stackrel{.}{v}}_x-a_x}{g}\right)\approx \frac{{\stackrel{.}{v}}_x-a_x}{g}\\ \phi =\arcsin \left(\frac{a_y-{\omega }_z{v}_x-{\stackrel{.}{v}}_y}{g\cos \theta }\right)\approx \frac{a_y-{\omega }_z{v}_x}{g\cos \theta }\end{array}---\left(6\right)$

由式（6）可知，只需测得车辆的纵向速度、纵向加速度、侧向加速度与横摆角速度， 即可利用所建立并合理简化后的车辆行驶动力学方程（3）来估计车辆的俯仰角和侧倾角； 因此，仅需要两个低成本MEMS（Micro-Electro-Mechanic System，微机电系统）加速度 传感器，一个低成本MEMS陀螺仪以及车速传感器即可满足测量要求，；

其中两个低成本MEMS加速度传感器安装于车辆质心位置附近，一个沿车体坐标系 纵轴，用以测量纵向加速度，一个沿车体坐标系横轴，用以测量侧向加速度，低成本MEMS 陀螺仪也安装于车辆质心位置附近，沿车体坐标系垂向轴安装，用以测量横摆角速度，车 速传感器用于测量纵向车速，霍尔车速传感器或轮速传感器等传感器均可采用，在此不做 限定，但要求车速测量精度误差小于0.05米/秒，在测得纵向车速信号后，并将其对时间 求导即可获取其微分；

事实上，若车辆安装有电子稳定控制或横摆稳定控制系统，则这些信息可以通过车辆 的CAN（Controller Area Network，控制器局域网络）总线获取。

将式（6）表示为参数识别标准形式：

式（7）中，k表示离散时刻， <math> <mrow> <mi>&gamma;</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mover> <mi>&theta;</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mover> <mi>&phi;</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </math> 表示待估参数矩阵，其中，与分别表示
待估计的车辆俯仰角与侧倾角； <math> <mrow> <mi>y</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mover> <mi>v</mi> <mo>.</mo> </mover> <mrow> <mi>x</mi> <mo>_</mo> <mi>m</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>x</mi> <mo>_</mo> <mi>m</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>a</mi> <mrow> <mi>y</mi> <mo>_</mo> <mi>m</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>&omega;</mi> <mrow> <mi>z</mi> <mo>_</mo> <mi>m</mi> </mrow> </msub> <msub> <mi>v</mi> <mrow> <mi>x</mi> <mo>_</mo> <mi>m</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </math> 表示系统输出矩阵，a_{x_m}、a_{y_m}与
ω_{z_m}分别表示利用低成本MEMS传感器所测得的纵向加速度、侧向加速度与横摆角速度，
v_{x_m}表示通过车速传感器所获取的车辆纵向速度，表示v_{x_m}的微分，由车速传感器所
测得的纵向速度信号对时间求导来获取，即在每一离散时刻k，有：

${\stackrel{.}{v}}_{x\_m}\left(k\right)=\frac{v_{x\_m}\left(k\right)-v_{x\_m}(k-1)}{\mathrm{dt}}---\left(8\right)$

式（8）中，dt表示采样时间间隔，本发明中，dt=0.01（秒）；
表示输入回归矩阵，本发明中上角标^T表示对矩阵转置；则利用带遗忘因子的递归最小二
乘（Recursive Least Squares,RLS）算法实时估计车辆侧倾角与横摆角的估计步骤如下：

（1）计算系统输出矩阵y(k)，并计算输入回归矩阵

（2）计算增益矩阵K(k)；其中，方差矩阵
参数λ为遗忘因子，能有效减少不再与模型相关的旧数
据的影响，并防止协方差发散，通常取值范围在[0.9,1]，本发明取0.975；

（3）计算待估参数矩阵γ(k)；

其中I为2×2单位矩阵，至此，可实时估计出车辆侧倾角与横摆角。

实施实例2

为检验本发明提出的基于递归最小二乘的车辆侧倾角和俯仰角估计方法的实际效果， 在专业的汽车动力学仿真软件CarSim上进行了仿真验证实验。

CarSim是由美国MSC(Mechanical Simulation Corporation)公司开发的专门针对车辆动 力学的仿真软件，目前已被国际上众多的汽车制造商、零部件供应商所采用，被广泛地应 用于现代汽车控制系统的商业开发，已成为汽车行业的标准软件，享有很高的声誉。Carsim 内的车辆动力学模型是通过分别对汽车的车体、悬架、转向、制动等各子系统以及各个轮 胎的高逼真建模来实现的，具有很高的自由度，能够提供非常接近实际的准确的车辆运行 状态信息，因此，Carsim输出的车辆运行状态信息可作为车辆的参考输出。

为检验本发明提出的算法在车辆典型行驶工况下的估计效果，在仿真试验中设置了两 个典型工况，工况中包含了车辆直线和曲线运动，道路坡度的变化和车速的变化，工况具 体描述见表1。

表1两种典型仿真工况

仿真所用车辆是一个前轮转向的四轮小型客车，获取车辆纵向车速采用轮速传感器， 所需惯性传感器和轮速传感器的采样频率都为100Hz，对于基于MEMS的低成本惯性传 感器，陀螺仪的零均值随机白噪声的标准差为0.2（度/秒），加速度传感器的零均值随机 白噪声的标准差为0.1956（米/（秒×秒）），轮速传感器的测量噪声均为均值是0、标准 差是0.05（米/秒）的高斯白噪声。仿真初值设置如下：方差矩阵初值 $P=\left[\begin{array}{cc}10& 0\\ 0& 10\end{array}\right],$ 待估矩阵初值为 $\gamma \left(0\right)=\left[\begin{array}{c}0\\ 0\end{array}\right].$

表2和图1、图2给出了仿真实验的结果。表2列出了利用直测法和本发明方法推算 车辆侧倾角和俯仰角的统计结果对比，表中的误差均是相对于Carsim输出的相应参考值 而言的（如直测法的俯仰角误差就表示利用直测法推算的俯仰角相对于Carsim输出的俯 仰角参考值的误差）。另外需指出的是，上述两种方法的具体含义如下：直测法是指利用 惯性传感器输出值，通过实施实例1中式（6）直接推算得到俯仰角和侧倾角的方法；本 发明方法是指利用本发明提出的递归最小二乘估计方法来推算车辆侧倾角和俯仰角的方 法。

表2两种方法推算侧倾角与俯仰角效果对比表（单位：deg）

图1给出了工况1中利用直测法和本发明方法估计的俯仰角的结果曲线（图中以 Original灰虚线表示直测法结果，以RLS黑点线标示本发明方法估计结果），以及相 应的Carsim的参考输出值（图中以Carsim实黑线标示），图2给出了工况2中利用直测 法和本发明方法估计的侧倾角的结果曲线（图中以Original灰虚线表示直测法结果，以 RLS黑点线标示本发明方法估计结果），以及相应的Carsim的参考输出值（图中以 Carsim实黑线标示）。

由表2的对比以及图1～图2，可以看出本发明方法相对于直测法在侧倾角和俯仰角 的推算方面精度有了大幅的提高。