CN104034332A - 一种基于卡尔曼滤波的救援清障车姿态角估计方法 - Google Patents

Abstract

本发明公开了一种基于卡尔曼滤波的救援清障车姿态角估计方法。本方法通过建立救援清障车的动力学模型，根据其工作特点，简化侧倾角和俯仰角的计算公式，并利用卡尔曼滤波算法获得救援清障车侧倾角与俯仰角的估计值。本发明方法使用较少的传感器，且可以用于在复杂救援环境下估计较大角度的侧倾角与俯仰角，具有精度高、成本低、实时性好、适用范围广等显著优点。可用于救援清障车姿态监控及危险姿态预警。

Description

一种基于卡尔曼滤波的救援清障车姿态角估计方法

技术领域

本发明涉及一种救援清障车姿态角估计方法，尤其涉及一种基于卡尔曼滤波的救援清障车姿态角估计方法，其目的在于为救援清障车提供姿态监控及危险姿态预警，属于救援清障车安全预警领域。

背景技术

近几年，为了解决道路交通事故发生后的清障救援问题，我国的救援清障车得到了高速发展。与此同时，根据有关部门的统计，救援清障车发生侧翻或倾翻的事故率也逐年增加，往往存在着未能安全救起被救车辆，而自身却发生事故的情况。究其原因，除了存在操作者违章作业、经验不足和管理不善等主观因素外，救援清障车的姿态监控装置尚不完善，无法为驾驶员提供准确的姿态信息以及安全预警也是不可回避的客观因素。

对于普通车辆而言，通常使用一个完整的六维惯性测量单元IMU(InertialMeasurementUnit)来确定侧倾角和俯仰角等姿态角信息，该IMU包括3个加速度计和3个角速度陀螺仪，车辆的姿态角信息可以通过六维IMU的捷联导航算法推算得出。此方案用于救援清障车时，依然可以保证很高的精度和实时性。然而，六维IMU价格昂贵，特别是三个陀螺仪的价格。由于救援清障车在工作时，仅利用吊臂开展施救工作，而车身相对于地面处于静止状态，利用这一工作特点可以对所建立的动力学方程进行合理简化，从而减少所需的传感器数量，降低成本。如果使用MEMS(Micro-Electro-MechanicSystem，微机电系统)传感器，可以使成本进一步降低，利于设备的大规模推广。

专利“一种救援清障车姿态角估计方法”(公开号：CN103616013A)提出了一种基于递归最小二乘(RLS)的救援清障车姿态角估计方法，该方法填补了国内该领域的空白，并且使用了MEMS传感器，具有成本低、实时性好等优点。但是该方法仅考虑路面横向坡度率与纵向坡度率小于20％的工况，此时俯仰角和侧倾角都较小，所以在估计过程中采用了约等于处理。但是考虑到在复杂救援环境下，路面的横向坡度率与纵向坡度率可能会达到30％。在一些特殊路段上，路面的横向坡度率与纵向坡度率甚至会超过55％。这些情况下，俯仰角和侧倾角较大，是不能进行约等于处理的，上述方法在实际应用过程中会受到限制。

发明内容

本发明的目的是提出一种基于卡尔曼滤波的救援清障车姿态角估计方法，该方法使用的传感器数量少，且可以估算出较大角度的俯仰角和侧倾角，具有精度高、成本低、实时性好、适用范围广的特点，可以为救援清障车提供姿态监控以及危险姿态预警。

本发明采用的技术方案如下：一种基于卡尔曼滤波的救援清障车姿态角估计方法，其特征在于：本发明根据救援清障车工作特点，对其进行动力学建模，进一步通过卡尔曼滤波算法实现对救援清障车侧倾角与俯仰角等姿态角的实时、准确估计，该方法可以用于在复杂救援环境下估计较大角度的俯仰角和侧倾角，且仅需两个低成本MEMS车载加速度传感器。具体步骤包括：

1)建立救援清障车的动力学模型

由于救援清障车在工作时的俯仰角速度、侧倾角速度与垂向速度均为零，且忽略地球旋转速度，则可建立救援清障车的动力学方程：

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_y-g\sin \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(1\right)$

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

由于救援清障车在工作时，仅利用吊臂开展施救工作，车身相对于地面处于静止状态，即可以合理地认为v_x、v_y、和ω_z均为零，则式(1)可以简化为：

$\begin{array}{c}{a}_x+g\sin \mathrm{\θ}=0\\ a_y-g\sin \mathrm{\φ}\cos \mathrm{\θ}=0\end{array}---\left(2\right)$

由式(2)可得

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{-a_x}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y}{g\cos \mathrm{\θ}}\right)\end{array}---\left(3\right)$

2)所需车载传感器安装

由式(3)可知，只需测得车辆纵向加速度与横向加速度，即可对救援清障车的俯仰角和侧倾角进行估计。因此，仅需要两个低成本MEMS加速度传感器即可满足测量要求。两个低成本MEMS加速度传感器安装于车身(即除吊臂外救援清障车辆的其他部分)质心位置附近，一个与车体坐标系纵轴平行，用以测量纵向加速度，另一个与车体坐标系横轴平行，用以测量成横向加速度；

3)建立卡尔曼滤波的状态方程和观测方程：

离散化后的卡尔曼滤波状态方程的矩阵形式为：

式(4)中，k表示离散化时刻；系统状态向量为X＝[x₁ x₂]′且x₁＝θ，x₂＝φ，即X＝[θ φ]′，本发明中上角标'表示对矩阵转置；W(k-1)表示零均值的系统高斯白噪声向量且W＝[w₁ w₂]′，其中w₁、w₂分别表示两个系统高斯白噪声分量，W(k-1)对应的系统噪声协方差阵Q(k-1)为： $Q(k-1)=\left[\begin{array}{cc}{\mathrm{\σ}}_{w_1}^2& 0\\ 0& {\mathrm{\σ}}_{w_2}^2\end{array}\right],$ 其中σ_w1 ²、σ_w2 ²分别表示系统高斯白噪声w₁、w₂对应的方差；状态转移矩阵为这是因为救援清障车在工作过程中侧倾角与俯仰角是基本保持不变的，可以认为上一采样时刻的侧倾角与俯仰角等于下一采样时刻的侧倾角与俯仰角。

离散化的卡尔曼滤波观测方程的矩阵形式为：

Z(k)＝H(k)·X(k)+V(k) (5)

式(5)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量。由于观测向量与状态向量都是指侧倾角与俯仰角，所以 $H\left(k\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ $Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\φ}}_m\left(k\right)\end{array}\right],$ $V=\left[\begin{array}{c}{n_{\mathrm{\θ}}}_m\\ {n_{\mathrm{\φ}}}_m\end{array}\right],$ 其中θ_m(k)和φ_m(k)分别为通过传感器测量值直接推算得出的俯仰角与侧倾角值，根据式(3)，有：

$\begin{array}{c}{\mathrm{\θ}}_m=\arcsin \left(\frac{-a_{x\_m}}{g}\right)\\ {\mathrm{\φ}}_m=\arcsin \left(\frac{a_{y\_m}}{g\cos {\mathrm{\θ}}_m}\right)\end{array}---\left(6\right)$

式(6)中，a_{x_m}、a_{y_m}分别表示利用低成本MEMS传感器所测得的纵向加速度、横向加速度；表示由式(6)计算获得的车辆俯仰角的观测噪声且是均值为0、方差为的高斯白噪声，表示通过式(6)计算获得的车辆侧倾角的的观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为 $R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{{\mathrm{\θ}}_m}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\φ}}_m}}^2\end{array}\right];$

4)卡尔曼滤波算法：

对于式(4)和式(5)所描述的系统状态方程和测量方程，建立标准的卡尔曼滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程

一步预测误差方差阵

测量更新：

滤波增益矩阵K(k)＝P(k,k-1)·H^T(k)·[H(k)P(k,k-1)H′(k)+R(k)]^-1

状态估计 $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\·\hat{X}(k,k-1)]$

估计误差方差阵P(k)＝[I-K(k)·H(k)]·P(k,k-1)

经过上述递推计算后，可实时估计出救援清障车在每个离散时刻k的侧倾角与横摆角。

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

为了解决上述技术在应用中的不足，本发明提出一种基于卡尔曼滤波的救援清障车俯仰角和侧倾角估算方法。卡尔曼滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值进行递推计算，便可实现对实时信号的估计，具有数据存储量小、算法简便的特点。本发明提出的方法可以用于在复杂工况下估计较大角度的俯仰角和侧倾角，适用范围更广，为救援清障车提供更好的姿态监控及危险姿态预警。

(1)本发明提出了一种低成本、高精度、实时性好、适用范围广的救援清障车侧倾角和俯仰角估计方法，可以估算出较大角度的侧倾角和俯仰角，可用于救援清障车在复杂工况下的姿态监控和危险姿态预警；

(2)本发明根据救援清障车的工作特点，对其动力学模型进行合理简化后，建立卡尔曼滤波的系统状态方程和观测方程，并利用卡尔曼滤波算法进行侧倾角和俯仰角的估计，估计结果可以满足实际应用的精度与实时性要求

(3)本发明仅需两个低成本MEMS加速度传感器，具有成本低的优点，便于大规模推广。

附图说明

图1是俯仰角估计仿真结果。

图2是侧倾角估计仿真结果。

具体实施方式

实施实例1

近几年，为了解决道路交通事故发生后的清障救援问题，我国的救援清障车得到了高速发展。与此同时，根据有关部门的统计，救援清障车发生侧翻或倾翻的事故率也逐年增加，往往存在着未能安全救起被救车辆，而自身却发生事故的情况。究其原因，除了存在操作者违章作业、经验不足和管理不善等主观因素外，救援清障车的姿态监控装置尚不完善，无法为驾驶员提供准确的姿态信息以及安全预警也是不可回避的客观因素。专利“一种救援清障车姿态角估计方法”(公开号：CN103616013A)提出了一种基于递归最小二乘(RLS)的救援清障车姿态角估计方法，该方法填补了国内该领域的空白，具有成本低、实时性好等优点。但是该方法仅在救援清障车的俯仰角和侧倾角较小时才有效，即不能准确估算出较大角度的俯仰角和侧倾角，应用工况受限。

对于普通车辆，通常用来确定侧倾角和俯仰角等姿态角信息的方法是使用完整的六维惯性测量单元IMU(InertialMeasurementUnit)，该IMU包括3个加速度计和3个角速度陀螺仪，利用IMU输出量和角度信息的微分之间的运动学关系，并忽略地球旋转速度，车辆动力学过程可建模为[此处可参考文献：H.EricTseng,LiXu，DavorHrovat，Estimationoflandvehiclerollandpitchangles[J].VehicleSystemDynamics:InternationalJournalofVehicleMechanicsandMobility,2007,45(5):433-443.]：

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y-{\mathrm{\ω}}_y{v}_z+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_x+{\mathrm{\ω}}_x{v}_z-g\sin \mathrm{\φ}\cos \mathrm{\θ}\\ {\stackrel{\·}{v}}_z=a_z+{\mathrm{\ω}}_y{v}_x-{\mathrm{\ω}}_x{v}_y-g\cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(1\right)$

式中，ω_x,ω_y和ω_z分别表示围绕车体坐标系纵轴、横轴以及垂向轴的角速度，v_x,v_y和v_z分别表示沿车体坐标系纵轴、横轴以及垂向轴的线速度，a_x,a_y和a_z分别表示沿车体坐标系纵轴、横轴以及垂向轴的加速度；φ,θ,和ψ分别表示侧倾、俯仰和横摆三个欧拉角；g表示重力加速度；上标志“·”表示微分，如表示v_x的微分。。

由式(1)可得，车辆的姿态角信息可以通过六维IMU的导航解算算法得出，这一类方法在大量车辆定位文献中都有涉及。然而，六维IMU价格昂贵，特别是三个陀螺仪的价格。虽然MEMS(Micro-Electro-Mechanic System，微机电系统)传感器的测量精度相比于传统传感器较低，但是由于其具有成本低的突出特点，应用依然越来越广泛。本发明结合救援清障车的工作特点，研究如何利用尽量少的MEMS传感器来估计救援清障车的侧倾角和俯仰角，并达到较高的精度要求。

由于救援清障车在工作时，仅利用吊臂开展施救工作，车身相对于地面处于静止状态，即可以合理地认为v_x、v_y、v_z、ω_x、ω_y和ω_z均为零，则式(1)可以简化为：

$\begin{array}{c}{a}_x+g\sin \mathrm{\θ}=0\\ a_y-g\sin \mathrm{\φ}\cos \mathrm{\θ}=0\end{array}---\left(2\right)$

由式(2)可得俯仰角与侧倾角的计算公式：

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{-a_x}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y}{g\cos \mathrm{\θ}}\right)\end{array}---\left(3\right)$

由式(3)可以看出，只需测得车辆纵向加速度与横向加速度，即可对救援清障车的俯仰角和侧倾角进行估计。因此，仅需要两个低成本MEMS加速度传感器即可满足测量要求。两个低成本MEMS加速度传感器安装于车身(即除吊臂外救援清障车辆的其他部分)质心位置附近，一个与车体坐标系纵轴平行，用以测量纵向加速度，另一个与车体坐标系横轴平行，用以测量成横向加速度。

路面的纵向和横向坡度是影响车辆侧倾角与俯仰角的主要因素，纵向和横向坡度越大，侧倾角与俯仰角也越大。一般路面的纵向和横向坡度都较小，进行侧倾角与俯仰角求解时，可以进行约等于处理，即：

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{-a_x}{g}\right)\≈-\frac{a_x}{g}\\ \mathrm{\φ}=\arcsin \left(\frac{a_y}{g\cos \mathrm{\θ}}\right)\≈\frac{a_y}{g\cos \mathrm{\θ}}\end{array}---\left(4\right)$

但是考虑到在复杂救援环境下，路面的横向坡度率与纵向坡度率可能会达到30％。在一些特殊路段上，路面的横向坡度率与纵向坡度率甚至会超过55％。这些情况下，侧倾角与俯仰角都较大，求解arcsin运算时，不能进行约等于处理。本发明中就没有对arcsin运算进行约等于处理，所以将本发明方法应用于复杂救援环境时，依然可以保证侧倾角与俯仰角估计的准确性。

卡尔曼滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值进行递推计算，便可实现对实时信号的估计，具有数据存储量小、算法简便的特点。将该方法应用于救援清障车的俯仰角和侧倾角估计时，在保证实时性的同时，可以有效提高估计精度。下面建立卡尔曼滤波的状态方程和观测方程。

离散化后的卡尔曼滤波的状态方程的矩阵形式表示为：

式(5)中，k表示离散化时刻；系统状态向量为X＝[x₁ x₂]′且x₁＝θ，x₂＝φ，即X＝[θ φ]′，本发明中上角标'表示对矩阵转置；W(k-1)表示零均值的系统高斯白噪声向量且W＝[w₁ w₂]′，其中w₁、w₂分别表示两个系统高斯白噪声分量，W(k-1)对应的系统噪声协方差阵Q(k-1)为： $Q(k-1)=\left[\begin{array}{cc}{\mathrm{\σ}}_{w_1}^2& 0\\ 0& {\mathrm{\σ}}_{w_2}^2\end{array}\right],$ 其中σ_w1 ²、σ_w2 ²分别表示系统高斯白噪声w₁、w₂对应的方差；状态转移矩阵为这是因为救援清障车在工作过程中侧倾角与俯仰角是基本保持不变的，可以认为上一采样时刻的侧倾角与俯仰角等于下一采样时刻的侧倾角与俯仰角；

卡尔曼滤波观测方程的离散化矩阵形式为：

Z(k)＝H(k)·X(k)+V(k) (6)

式(6)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量。由于观测向量与状态向量都是指侧倾角与俯仰角，所以 $H\left(k\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ $Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\φ}}_m\left(k\right)\end{array}\right],$ $V=\left[\begin{array}{c}{n_{\mathrm{\θ}}}_m\\ {n_{\mathrm{\φ}}}_m\end{array}\right],$ 其中θ_m(k)和φ_m(k)分别为通过传感器测量值直接推算得出的俯仰角与侧倾角值，根据式(3)，有：

$\begin{array}{c}{\mathrm{\θ}}_m=\arcsin \left(\frac{-a_{x\_m}}{g}\right)\\ {\mathrm{\φ}}_m=\arcsin \left(\frac{a_{y\_m}}{g\cos {\mathrm{\θ}}_m}\right)\end{array}---\left(7\right)$

式(7)中，a_{x_m}、a_{y_m}分别表示利用低成本MEMS传感器所测得的纵向加速度、横向加速度；表示由式(7)计算获得的车辆俯仰角的观测噪声且是均值为0、方差为的高斯白噪声，表示通过式(7)计算获得的车辆侧倾角的的观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为 $R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{{\mathrm{\θ}}_m}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\φ}}_m}}^2\end{array}\right];$

对于式(5)和式(6)所描述的系统状态方程和测量方程，运用卡尔曼滤波理论，建立下面的标准滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程

一步预测误差方差阵

测量更新：

滤波增益矩阵K(k)＝P(k,k-1)·H^T(k)·[H(k)P(k,k-1)H′(k)+R(k)]^-1

状态估计 $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\·\hat{X}(k,k-1)]$

估计误差方差阵P(k)＝[I-K(k)·H(k)]·P(k,k-1)

经过上述递推计算后，可实时估计出救援清障车在每个离散时刻k的侧倾角与横摆角。

实施实例2

为检验本发明提出的基于卡尔曼滤波的救援清障车侧倾角和俯仰角估计方法的效果，在Matlab中进行仿真试验。

为了验证本发明方法对于大角度俯仰角和侧倾角的适用性，选取俯仰角和侧倾角分别为30°和-30°。纵向加速度和横向加速度的测量噪声均为均值是0、标准差是50mg的高斯白噪声。

表1和图1、图2给出了仿真实验的结果。表1列出了利用直测法和本发明方法推算车辆侧倾角和俯仰角的统计结果对比，表中的误差均是相对于设定值而言的。另外需指出的是：直测法是指用惯性传感器输出的包含噪声的值，通过实施实例1中式(7)直接计算得到俯仰角和侧倾角的方法；本发明方法是指利用本发明提出的基于卡尔曼滤波来推算车辆侧倾角和俯仰角的方法。

表1 两种方法推算侧倾角与俯仰角效果对比表(单位：deg)

图1给出了利用直测法和本发明方法估计的俯仰角的结果曲线,图中以灰色虚线表示直测法结果，以黑色实线表示本发明方法估计结果。图2给出了利用直测法和本发明方法估计的侧倾角的结果曲线，图中以灰色虚线表示直测法结果，以黑色实线表示本发明方法估计结果。

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

Claims (1)

1.一种基于卡尔曼滤波的救援清障车姿态角估计方法，其特征在于：本发明根据救援清障车工作特点，对其进行动力学建模，进一步通过卡尔曼滤波算法实现对救援清障车侧倾角与俯仰角等姿态角的实时、准确估计，该方法可以用于在复杂救援环境下估计较大角度的俯仰角和侧倾角，且仅需两个低成本MEMS车载加速度传感器，具体步骤包括：

1)建立救援清障车的动力学模型

由于救援清障车在工作时的俯仰角速度、侧倾角速度与垂向速度均为零，且忽略地球旋转速度，则可建立救援清障车的动力学方程：

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_y-g\sin \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(1\right)$

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

由于救援清障车在工作时，仅利用吊臂开展施救工作，车身相对于地面处于静止状态，即可以合理地认为v_x、v_y、和ω_z均为零，则式(1)可以简化为：

$\begin{array}{c}{a}_x+g\sin \mathrm{\θ}=0\\ a_y-g\sin \mathrm{\φ}\cos \mathrm{\θ}=0\end{array}---\left(2\right)$

由式(2)可得

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{-a_x}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y}{g\cos \mathrm{\θ}}\right)\end{array}---\left(3\right)$

2)所需车载传感器安装

由式(3)可知，只需测得车辆纵向加速度与横向加速度，即可对救援清障车的俯仰角和侧倾角进行估计，因此，仅需要两个低成本MEMS加速度传感器即可满足测量要求，两个低成本MEMS加速度传感器安装于车身(即除吊臂外救援清障车辆的其他部分)质心位置附近，一个与车体坐标系纵轴平行，用以测量纵向加速度，另一个与车体坐标系横轴平行，用以测量成横向加速度；

3)建立卡尔曼滤波的状态方程和观测方程：

离散化后的卡尔曼滤波状态方程的矩阵形式为：

式(4)中，k表示离散化时刻；系统状态向量为X＝[x₁ x₂]′且x₁＝θ，x₂＝φ，即X＝[θ φ]′，本发明中上角标'表示对矩阵转置；W(k-1)表示零均值的系统高斯白噪声向量且W＝[w₁ w₂]′，其中w₁、w₂分别表示两个系统高斯白噪声分量，W(k-1)对应的系统噪声协方差阵Q(k-1)为： $Q(k-1)=\left[\begin{array}{cc}{\mathrm{\σ}}_{w_1}^2& 0\\ 0& {\mathrm{\σ}}_{w_2}^2\end{array}\right],$ 其中σ_w1 ²、σ_w2 ²分别表示系统高斯白噪声w₁、w₂对应的方差；状态转移矩阵为这是因为救援清障车在工作过程中侧倾角与俯仰角是基本保持不变的，可以认为上一采样时刻的侧倾角与俯仰角等于下一采样时刻的侧倾角与俯仰角，

离散化的卡尔曼滤波观测方程的矩阵形式为：

Z(k)＝H(k)·X(k)+V(k) (5)

式(5)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量，由于观测向量与状态向量都是指侧倾角与俯仰角，所以 $H\left(k\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ $Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\φ}}_m\left(k\right)\end{array}\right],$ $V=\left[\begin{array}{c}{n_{\mathrm{\θ}}}_m\\ {n_{\mathrm{\φ}}}_m\end{array}\right],$ 其中θ_m(k)和φ_m(k)分别为通过传感器测量值直接推算得出的俯仰角与侧倾角值，根据式(3)，有：

$\begin{array}{c}{\mathrm{\θ}}_m=\arcsin \left(\frac{-a_{x\_m}}{g}\right)\\ {\mathrm{\φ}}_m=\arcsin \left(\frac{a_{y\_m}}{g\cos {\mathrm{\θ}}_m}\right)\end{array}---\left(6\right)$

式(6)中，a_{x_m}、a_{y_m}分别表示利用低成本MEMS传感器所测得的纵向加速度、横向加速度；表示由式(6)计算获得的车辆俯仰角的观测噪声且是均值为0、方差为的高斯白噪声，表示通过式(6)计算获得的车辆侧倾角的的观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为 $R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{{\mathrm{\θ}}_m}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\φ}}_m}}^2\end{array}\right];$

4)卡尔曼滤波算法：

对于式(4)和式(5)所描述的系统状态方程和测量方程，建立标准的卡尔曼滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程

一步预测误差方差阵

测量更新：

滤波增益矩阵K(k)＝P(k,k-1)·H^T(k)·[H(k)P(k,k-1)H′(k)+R(k)]^-1

状态估计 $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\·\hat{X}(k,k-1)]$

估计误差方差阵P(k)＝[I-K(k)·H(k)]·P(k,k-1)

经过上述递推计算后，可实时估计出救援清障车在每个离散时刻k的侧倾角与横摆角。