＜论文阅读＞利用多个惯性测量单元（IMU）的轻巧而精确的定位算法

来自论文：A Lightweight and Accurate Localization Algorithm Using Multiple Inertial Measurement Units

一丶摘要

本文通过融合来自多个惯性测量单元（IMU）和传感器的信息，提出了一种新颖的惯性辅助定位方法。 IMU是一种低成本的运动传感器，可对移动平台的角速度和重力补偿的线性加速度进行测量，并广泛用于现代定位系统中。迄今为止，大多数现有的惯性辅助定位方法仅利用一个IMU。尽管单个IMU定位可以为不同的使用案例提供可接受的准确性和鲁棒性，但通过使用多个IMU可以进一步提高整体性能。为此，我们提出了一种轻量级且精确的算法，用于融合来自多个IMU和传感器的测量结果，该算法能够获得显着的性能提升，而不会产生额外的计算成本。为此，我们首先将所有IMU的测量概率映射到一个虚拟IMU上。该步骤通过使用最小二乘估计器进行随机估计以及IMU间旋转加速度的概率边缘化来执行。随后，还导出了虚拟IMU的状态和错误状态的传播模型，这使得可以使用基于经典滤波器或基于优化的传感器融合算法进行定位。最后，提供了来自仿真和真实世界测试的结果，这些结果表明，提出的算法以明显的优势胜过竞争算法。

二丶单一IMU测量方法

1.IMU测量方程$${\omega }_m={}^I\omega +b_g+n_g,n_g\backsim \mathcal{N}(0,{\sigma }_g^2{I}_3)\text{\hspace{1em}(1)}$$$$a_m={}^I{R}_G({}^{G}a-{}^{G}g)+b_a+n_a,n_a\backsim \mathcal{N}(0,{\sigma }_g^2{I}_3)\text{\hspace{1em}(2)}$$

其中${\omega }_m$和$a_m\in {\mathbb{R}}^3$是陀螺仪和加速度计的测量值，${}^I\omega$和${}^{G}a$分别代表IMU角速度和线加速度在坐标系$I$和$G$中表示，${}^I{R}_G$代表坐标系$G$和坐标系$I$之间的变换，$n_a$和$n_g$为高斯噪声，$b_g$和$b_a$为随机游走测量偏差模型，而${}^{G}g$为全局坐标系下已知的重力向量。

2.IMU状态向量和误差状态

IMU的状态定义为：$$x=[{}_I^G{\bar{q}}^T,b_g^T,{}^G{v}_I^T,b_a^T,{}^G{p}_I^T{]}^T\in {\mathbb{R}}^{16}\text{\hspace{1em}(3)}$$其中，${}_I^G{\bar{q}}^T\in {\mathbb{R}}^4$代表从IMU坐标系到全局坐标系的四元数变换，${}^G{p}_I$和${}^G{v}_I$分别代表IMU的位置和速度。

IMU的连续时间运动动力学描述为：$$\dot{x}=f(x,{\omega }_m,a_m)\text{\hspace{1em}(4)}$$其详细形式为：$${}^G{\dot{p}}_I(t)={}^G{v}_I(t),{\dot{b}}_g(t)=n_{wg}(t),{\dot{b}}_a(t)=n_{wa}(t)$$$${}^G{\dot{v}}_I(t)={}^G{a}_I(t)={}^I{R}_G^T(a_m-b_a-n_a)+{}^{G}g$$$${}_I^G\dot{\bar{q}}(t)=\frac{1}{2}\Omega ({}^I\omega (t){}_I^G\bar{q}(t))=\frac{1}{2}({\omega }_m-b_g-n_g){}_I^G\bar{q}(t)$$其中，$n_{wg}\backsim \mathcal{N}(0,{\sigma }_{wg}^2{I}_3),n_{wa}\backsim \mathcal{N}(0,{\sigma }_{wa}^2{I}_3)$$$\Omega ({}^I\omega )=\left[\begin{array}{cc}0& -{}^I{\omega }^T\\ {}^I\omega & -\lfloor {}^I\omega \rfloor \end{array}\right]\text{\hspace{1em}(8)}$$$\lfloor .\rfloor$代表反对称矩阵。

IMU的误差状态定义为：$$\tilde{x}=[{}^I{\tilde{\theta }}^T,{\tilde{b}}_g^T,{}^G{\tilde{v}}_I^T,{\tilde{b}}_a^T,{}^G{\tilde{p}}_I^T{]}^T\in {\mathbb{R}}^{15}\text{\hspace{1em}(9)}$$其中标准加性误差定义用于位置、速度和偏差。四元数局部误差${}^I\tilde{\theta }$的定义如下：$${}^I{R}_G\cong (I_3-\lfloor {}^I\tilde{\theta }\rfloor ){}^I{\hat{R}}_G\text{\hspace{1em}(10)}$$这里的${}^I{\hat{R}}_G$代表${}^I{R}_G$估计值。

3.IMU传播方程

为了实现概率估计，一般的方法是推导一个预测状态估计的非线性方程$\hat{x}$和一个线性化的描述误差状态的方程$\tilde{x}$。$$\dot{\hat{x}}=f(\hat{x},{\omega }_m,a_m)\text{\hspace{1em}(11)}$$$$\dot{\tilde{x}}=F\tilde{x}+G{n}_c\text{\hspace{1em}(12)}$$其中$F$和$G$表示连续时间线性化的状态转移矩阵和噪声雅可比矩阵，$n_c$是过程噪声。一旦定义以上两个公式， IMU测量值可以直接集成到不同类型的定位估计器中，用于运动估计。

三丶虚拟IMU

我们制定多重IMU算法的高级设计原则是在没有非概率假设的情况下，推导出符合以上两个公式结构的IMU传播方程。所提出的算法可以直接集成到各种现有的惯性辅助定位系统中，而无需任何复杂的软件重新设计。

依靠多个IMU信息产生一个“虚拟”IMU测量值，被用于代替上式中的${\omega }_m$和$a_m$，以下通过两个IMU的案例进行说明。

“虚拟”IMU测量值：假设IMU A和B给出了同步测量${\omega }_{mA}$， ${\omega }_{mB}$, $a_{mA}$和$a_{mB}$，以及他们的外参$({}^A{R}_B,{}^B{p}_A)$，假设虚拟IMU相对于IMU A的外参数为$({}^A{R}_V,{}^V{p}_A)$，对应的虚拟陀螺仪和加速度计测量值可以生成为：$${\omega }_{mV}=N^{+}{\omega }_m\text{\hspace{1em}(13)}$$$$a_{mV}=T(a_m-S({\omega }_{mV}))\text{\hspace{1em}(14)}$$其中，$$N=\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]，Y=\left[\begin{array}{c}{}^A{R}_V\lfloor {}^V{p}_A\rfloor \\ {}^B{R}_V\lfloor {}^V{p}_B\rfloor \end{array}\right]，T=(Z^{T}N{)}^{+}{Z}^T\text{\hspace{1em}(15)}$$$${\omega }_m=\left[\begin{array}{c}{\omega }_{mA}\\ {\omega }_{mB}\end{array}\right]，a_m=\left[\begin{array}{c}{a}_{mA}\\ a_{mB}\end{array}\right]，S(\cdot )=\left[\begin{array}{c}{}^A{R}_V\lfloor .\rfloor {}^{2V}{p}_A\\ {}^B{R}_V\lfloor .\rfloor {}^{2V}{p}_B\end{array}\right]\text{\hspace{1em}(16)}$$$Z^T$是$Y$的左零空间投影，即$Z^{T}Y=0$，$A^{+}$为矩阵$A$广义的逆矩阵：$$A^{+}=(A^{T}A{)}^{-1}{A}^T\text{\hspace{1em}(17)}$$在实践中,对于多个IMU的 $N=[{}^1{R}_V^T,{}^2{R}_V^T,...,{}^{n1}{R}_V^T{]}^T$，其广义逆矩阵$N^{+}$的计算可以简化为$N^{+}=N^T/n_I$。利用生成的虚拟IMU测量值${\omega }_{mV},a_{mV}$，可以不做任何修改而使用IMU状态估计的预测方程$\dot{\hat{x}}=f(\hat{x},{\omega }_m,a_m)$。

1.虚拟陀螺模型

首先，我们使用以下等式：$${}^A\omega ={}^A{R}_V{}^V\omega ,{}^B\omega ={}^B{R}_V{}^V\omega \text{\hspace{1em}(18)}$$结合式子${\omega }_m={}^I\omega +b_g+n_g,n_g\backsim \mathcal{N}(0,{\sigma }_g^2{I}_3)$可推到得到：$$\left[\begin{array}{c}{\omega }_{mA}\\ {\omega }_{mB}\end{array}\right]=\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]{}^V\omega +\left[\begin{array}{c}{b}_{gA}\\ b_{gB}\end{array}\right]+\left[\begin{array}{c}{n}_{gA}\\ n_{gB}\end{array}\right]\text{\hspace{1em}(19)}$$
根据IMU A和B的测量结果，虚拟IMU角速度的最佳估计为：$${}^V\hat{\omega }=arg\underset{\omega }{\min }{\Vert \begin{array}{c}\left[\begin{array}{c}\frac{{\omega }_{mA}}{{\sigma }_{gA}}\\ \frac{{\omega }_{mB}}{{\sigma }_{gB}}\end{array}\right]-\left[\begin{array}{c}\frac{1}{{\sigma }_{gA}}{}^A{R}_V\\ \frac{1}{{\sigma }_{gB}}{}^B{R}_V\end{array}\right]{}^V\omega -\left[\begin{array}{c}\frac{b_{gA}}{{\sigma }_{gA}}\\ \frac{bgB}{{\sigma }_{gB}}\end{array}\right]\end{array}\Vert }_2^2\text{\hspace{1em}(20)}$$为了简化分析，假设${\sigma }_{gA}={\sigma }_{gB}$，当使用相同类型的IMU时，假设也会很精确。求解上式得到线性最小二乘估计：$${}^V\hat{\omega }=N^{+}\left[\begin{array}{c}{\omega }_{mA}\\ {\omega }_{mB}\end{array}\right]-N^{+}\left[\begin{array}{c}{b}_{gA}\\ b_{gB}\end{array}\right]\text{\hspace{1em}(21)}$$其中，$N=\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]，N^{+}=(N^{T}N{)}^{-1}{N}^T$，然后带入上式Eq.19，可推导得到：$${}^V\hat{\omega }=N^{+}\left(\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]{}^V\omega +\left[\begin{array}{c}{b}_{gA}\\ b_{gB}\end{array}\right]+\left[\begin{array}{c}{n}_{gA}\\ n_{gB}\end{array}\right]\right)-N^{+}\left[\begin{array}{c}{b}_{gA}\\ b_{gB}\end{array}\right]$$$$\Rightarrow {}^V\hat{\omega }=N^{+}\left(\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]{}^V\omega +\left[\begin{array}{c}{n}_{gA}\\ n_{gB}\end{array}\right]\right)$$因为$N^{+}$为矩阵$N$的广义逆矩阵，因此：$N^{+}\ast N=E$，所以得到：$$\Rightarrow {}^V\hat{\omega }={}^V\omega +N^{+}\left[\begin{array}{c}{n}_{gA}\\ n_{gB}\end{array}\right]\text{\hspace{1em}(22)}$$通过以上两个式子，我们可以将虚拟陀螺仪的测量值，与原始IMU的测量值类似表示为：$${\omega }_{mV}={}^V\omega +b_{gV}+n_{gV},n_{gV}\backsim \mathcal{N}(0,Q_{gV})\text{\hspace{1em}(23)}$$${\omega }_{mV},b_{gV},n_{gV}$分别是“虚拟”IMU的测量值，偏置向量和噪声：$${\omega }_{mV}\stackrel{△}{=}{N}^{+}\left[\begin{array}{c}{\omega }_{mA}\\ {\omega }_{mB}\end{array}\right],b_{gV}\stackrel{△}{=}{N}^{+}\left[\begin{array}{c}{b}_{gA}\\ b_{gB}\end{array}\right],n_{gV}\stackrel{△}{=}{N}^{+}\left[\begin{array}{c}{n}_{gA}\\ n_{gB}\end{array}\right]\text{\hspace{1em}(24)}$$噪声协方差矩阵$Q_{gV}$可计算为：$$Q_{gV}={\sigma }_{gA}^2{N}^{+}{N^{+}}^T={\sigma }_{gA}^2{\left({\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]}^T\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]\right)}^{-1}=\frac{{\sigma }_{gA}^2}{2}{I}_3\text{\hspace{1em}(25)}$$根据Eq. 24, $b_{gV}$的连续时间动力学可以描述为：$${\dot{b}}_{gV}(t)=n_{wgV}(t),n(wgV)\backsim \mathcal{N}(0,Q_{wgV})\text{\hspace{1em}(26)}$$将上述噪声建模为：$$Q_{wgV}\stackrel{△}{=}{N}^{+}\left[\begin{array}{cc}{\sigma }_{wgA}^2{I}_3& 0\\ 0& {\sigma }_{wgB}^2{I}_3\end{array}\right](N^{+}{)}^T\text{\hspace{1em}(27)}$$与Eq. 25类似，Eq. 27也表明通过产生虚拟测量，相应的陀螺仪的偏置漂移较慢。

2.虚拟加速度计模型

与虚拟陀螺仪类似，为了推导虚拟加速度计的模型，我们从等式开始：$${}^G{p}_A={}^G{p}_V+{}^G{R}_V{}^V{p}_A\text{\hspace{1em}(28)}$$上式可理解为平移+旋转，对Eq.28求一阶导数和二阶导数可得：$${}^G{v}_A={}^G{v}_v+{}^G{R}_V\lfloor {}^V\omega \rfloor {}^V{p}_A$$$${}^G{a}_A={}^G{a}_V+{}^G{R}_V\lfloor {}^V\omega {\rfloor }^2{}^V{p}_A+{}^G{R}_V\lfloor {}^V\varphi \rfloor {}^V{p}_A$$其中${}^V\omega ,{}^V\varphi \in {\mathbb{R}}^3$为全局帧中虚拟IMU帧的角速度和角加速度。将EQ.2带入上式可得出：$$a_m={}^I{R}_G({}^{G}a-{}^{G}g)+b_a+n_a,n_a\backsim \mathcal{N}(0,{\sigma }_g^2{I}_3)\text{\hspace{1em}(2)}$$$$\Rightarrow a_{mA}={}^A{R}_G({}^G{a}_A-{}^{G}g)+b_{aA}+n_{aA}$$$$\Rightarrow a_{mA}-b_{aA}-n_{aA}={}^A{R}_G({}^G{a}_A-{}^{G}g)$$带入：${}^G{a}_A={}^G{a}_V+{}^G{R}_V(\lfloor {}^V\omega {\rfloor }^2{}^V{p}_A+\lfloor {}^V\varphi \rfloor {}^V{p}_A)$$$a_{mA}-b_{aA}-n_{aA}={}^A{R}_G({}^G{a}_V+{}^G{R}_V(\lfloor {}^V\omega {\rfloor }^2{}^V{p}_A+\lfloor {}^V\varphi \rfloor {}^V{p}_A)-{}^{G}g)$$$$\Rightarrow a_{mA}-b_{aA}-n_{aA}={}^A{R}_G({}^G{a}_V-{}^{G}g)+{}^A{R}_V(\lfloor {}^V\omega {\rfloor }^2{}^V{p}_A+\lfloor {}^V\varphi \rfloor {}^V{p}_A)$$且 $s_V\stackrel{△}{=}{}^V{R}_G({}^G{a}_V-{}^{G}g),a\lfloor b\rfloor =-b\lfloor a\rfloor$$$a_{mA}-b_{aA}-n_{aA}={}^A{R}_V(s_V+\lfloor {}^V\omega {\rfloor }^2{}^V{p}_A-\lfloor {}^V{p}_A\rfloor {}^V\varphi )\text{\hspace{1em}(30)}$$类似于计算EQ.20中$\omega$的最佳估计值，叠加来自两个IMU的加速度计测量值EQ.30：$${\hat{s}}_V=arg\underset{s_V}{\min }{\Vert \begin{array}{c}\left[\begin{array}{c}{a}_{mA}\\ a_{mB}\end{array}\right]-\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]s_V+Y{}^V\varphi -S({}^V\omega )-\left[\begin{array}{c}{b}_{aA}\\ b_{aB}\end{array}\right]\end{array}\Vert }_2^2\text{\hspace{1em}(32)}$$其中，矩阵$Y$和运算符$S(\cdot )$在EQ.15和EQ.16处定义。在上式EQ.32中，旋转加速度${}^V\varphi$是未知的，并且是无法直接由IMU传感器测量得到，如果${}^V\varphi$不能被正确的得到，$s_V$也不能准确的表示。由未知的视觉特征被概率地边缘化所启发，通过$Z^T$左零空间投影，EQ.32等价于：$${\hat{s}}_V=arg\underset{s_V}{\min }\times {\Vert \begin{array}{c}{Z}^T\left(\left[\begin{array}{c}{a}_{mA}\\ a_{mB}\end{array}\right]-\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]s_V-S({}^V\omega )-\left[\begin{array}{c}{b}_{aA}\\ b_{aB}\end{array}\right]\right)\end{array}\Vert }_2^2\text{\hspace{1em}(33)}$$$$0=Z^T\left(\left[\begin{array}{c}{a}_{mA}\\ a_{mB}\end{array}\right]-\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]{\hat{s}}_V-S({}^V\omega )-\left[\begin{array}{c}{b}_{aA}\\ b_{aB}\end{array}\right]\right)$$$$Z^T\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]{\hat{s}}_V=Z^T\left(\left[\begin{array}{c}{a}_{mA}\\ a_{mB}\end{array}\right]-S({}^V\omega )-\left[\begin{array}{c}{b}_{aA}\\ b_{aB}\end{array}\right]\right)，T=(Z^{T}N{)}^{+}{Z}^T$$$${\hat{s}}_V={\left(Z^T\left[\begin{array}{c}{}^A{R}_V\\ {}^B{R}_V\end{array}\right]\right)}^{+}{Z}^T\left(\left[\begin{array}{c}{a}_{mA}\\ a_{mB}\end{array}\right]-S({}^V\omega )-\left[\begin{array}{c}{b}_{aA}\\ b_{aB}\end{array}\right]\right)$$$$\Rightarrow {\hat{s}}_V=T\left(\left[\begin{array}{c}{a}_{mA}\\ a_{mB}\end{array}\right]-S({}^V\omega )-\left[\begin{array}{c}{b}_{aA}\\ b_{aB}\end{array}\right]\right)\text{\hspace{1em}(34)}$$其中$T=(Z^{T}N{)}^{+}{Z}^T$，$Z^T$可以通过QR分解来计算，结合：$$a_{mV}=T(a_m-S({\omega }_{mV}))$$$$a_{mA}-b_{aA}-n_{aA}={}^A{R}_V(s_V+\lfloor {}^V\omega {\rfloor }^2{}^V{p}_A-\lfloor {}^V{p}_A\rfloor {}^V\varphi )$$$$\Rightarrow a_{mV}=s_V+b_{aV}+n_{aV}-T\cdot S_a\text{\hspace{1em}(35)}$$$$b_{aV}\stackrel{△}{=}T\left[\begin{array}{c}{b}_{aA}\\ b_{aB}\end{array}\right],n_{aV}\stackrel{△}{=}T\left[\begin{array}{c}{n}_{aA}\\ n_{aB}\end{array}\right]\text{\hspace{1em}(36)}$$$$S_a=S({\omega }_{mV}-{}^V\omega )=\left[\begin{array}{c}{}^A{R}_V\zeta {}^V{p}_A\\ {}^B{R}_V\zeta {}^V{p}_B\end{array}\right],\zeta =\lfloor {\omega }_{mV}{\rfloor }^2-\lfloor {}^V\omega {\rfloor }^2\text{\hspace{1em}(37)}$$虚拟加速度计的测量会受到陀螺仪偏差、加速度计偏差、陀螺仪噪声和原加速度计噪声的影响。这不同于原始的IMU测量方程，即Eq. 1和2，在这些方程中，测量噪声是不相关的，偏差是独立的。

3.虚拟IMU传播

在本节中，提出了一种集成虚拟IMU测量的方法，对于IMU状态和误差，即公式13和14。与单IMU的情况类似，即公式3和9，我们定义虚拟IMU的状态和误差为：$$x_V=[{{}^G}_V{\bar{q}}^T,b_{gV}^T,{}^G{v_V}^T,b_{aV}^T,{}^G{p_V}^T{]}^T\in {\mathbb{R}}^{16}\text{\hspace{1em}(40)}$$$$\tilde{x}=[{}^V{\tilde{\theta }}^T,{\tilde{b}}_{gV}^T,{}^G{{\tilde{v}}_V}^T,{\tilde{b}}_{aV}^T,{}^G{\widetilde{p_V}}^T{]}^T\in {\mathbb{R}}^{15}\text{\hspace{1em}(41)}$$它的连续时间传播方程为：$${\dot{\hat{x}}}_V=f(\hat{x},{\omega }_{mV},a_{mV})\text{\hspace{1em}(42)}$$$${\dot{\tilde{x}}}_V=F_V{\tilde{x}}_V+G_V{n}_V\text{\hspace{1em}(43)}$$其中，$$n_V=[{n_{gV}}^T,{n_{wgV}}^T,{n_{aV}}^T,{n_{waV}}^T{]}^T\in {\mathbb{R}}^{12}\text{\hspace{1em}(44)}$$式42和43提供了基于虚拟IMU的执行状态和错误状态传播的核心方程。这种传播可以直接集成到不同类型的紧密耦合定位算法中，与使用单个IMU的过程相同。为了实现Eq. 42，我们首先通过Eq. 13和14生成所有IMU的虚拟IMU测量值，其余的位姿积分步骤与Eq.11相同。通过表示：$$\hat{\omega }={\omega }_{V_m}-{\hat{b}}_{gV},{\hat{a}}_V=a_{Vm}-{\hat{b}}_{aV}+T{\hat{S}}_a\text{\hspace{1em}(45)}$$状态转移矩阵和噪声雅可比矩阵可由Eq.43计算得出，详细的表达式如下：$$F_V=\left[\begin{array}{ccccc}-\lfloor \hat{\omega }\rfloor & -I_3& 0_3& 0_3& 0_3\\ 0_3& 0_3& 0_3& 0_3& 0_3\\ -{}^V{\hat{R}}_G^T\lfloor {\hat{a}}_V\rfloor & -{}^V{\hat{R}}_G^{T}T\Psi & 0_3& -{}^V{\hat{R}}_G^T& 0_3\\ 0_3& 0_3& 0_3& 0_3& 0_3\\ 0_3& 0_3& I_3& 0_3& 0_3\end{array}\right]$$$$G_V=\left[\begin{array}{cccc}-I_3& 0_3& 0_3& 0_3\\ 0_3& I_3& 0_3& 0_3\\ 0_3& -{}^V{\hat{R}}_G^{T}T\Xi & -{}^V{\hat{R}}_G^T& 0_3\\ 0_3& 0_3& 0_3& I_3\\ 0_3& 0_3& 0_3& 0_3\end{array}\right]$$与单一IMU情况相比，$F_V$和$G_V$增加了以下非零项：$$\frac{\partial {}^G{\dot{\tilde{v}}}_V}{\partial {\tilde{b}}_{gV}}=-{}^V{\hat{R}}_G^{T}T\Psi ,\frac{\partial {}^G{\dot{\tilde{v}}}_V}{\partial n_{gV}}=-{}^V{\hat{R}}_G^{T}T\Xi \text{\hspace{1em}(47)}$$其中，$$\Psi =\Xi =\left[\begin{array}{c}{}^A{R}_V(-\lfloor {}^V\hat{\omega }\rfloor \lfloor {}^V{p}_A\rfloor -\lfloor \lfloor {}^V\hat{\omega }\rfloor {}^V{p}_A\rfloor )\\ {}^B{R}_V(-\lfloor {}^V\hat{\omega }\rfloor \lfloor {}^V{p}_B\rfloor -\lfloor \lfloor {}^V\hat{\omega }\rfloor {}^V{p}_B\rfloor )\end{array}\right]\text{\hspace{1em}(48)}$$