CN101109959A - 一种适用于任意运动微小型系统的定姿系统 - Google Patents

Abstract

一种适用于任意运动微小型系统的定姿系统，其特征在于：包括微磁传感器MMS模块、微惯性测量单元MIMU模块、GPS模块和Kalman滤波模块，在微小型系统任意运动状态下，MIMU模块根据输出数据通过捷联导航算法分解出微小型系统重力加速度；MMS模块利用所述的重力加速度通过定姿算法实时解算微小型系统的横滚角、俯仰角和航向角三维姿态信息；同时，利用恒虚警CFAR滤波器平滑掉GPS输出速度和加速度信息中的噪声；三维姿态信息一方面送至Kalman滤波模块，与GPS给出的位置和速度信息一道通过信息融合算法间歇校正MIMU误差，另一方面利用三维姿态信息实时更新C_n ^b坐标转换矩阵；坐标转换矩阵C_n ^b将被送给MIMU模块和GPS模块，将水平坐标系的信息转换到载体坐标系下。本发明利用了GPS、MIMU、MMS之间的迭代解算关系，解决了任意运动状态下确定运动体航向的难题，具有广阔的应用前景。

Description

一种适用于任意运动微小型系统的定姿系统

技术领域

本发明涉及一种适用于任意运动微小型系统的定姿系统，适用于微小型系统任意运动状态下航向的确定。

背景技术

任何运动体都需要高精度导航系统和控制系统，高精度导航系统为运动体提供当前的位置、速度和姿态等运动信息；高精度控制系统依据导航系统提供的运动信息控制运动体按照要求的方式运动，其中姿态信息的精度对控制系统性能有决定性的影响。

大型运动体利用高精度、大体积、高成本惯性导航系统或者星敏感器已经实现了任意运动情况下的姿态确定。但大型运动体所用的定姿设备由于体积庞大、成本极高而无法应用于微小型系统任意运动情况下的姿态确定。微小型系统(微小飞行器、微小水下航行器、微小地面机器人)机动性大且随机性强，姿态确定的难度很大。

加速度计和倾角传感器是目前测量微小型系统横滚角和俯仰角最常用的方式，但由于两种传感器均无法很好的区别微小型系统运动加速度和重力加速度，因此不能给出微小型系统任意运动情况下的横滚角和俯仰角，只适合于静止和匀速直线运动情况。而且上述两种传感器均不能给出航向角。

惯性导航系统原理上能够测量微小型系统在任意运动状态下的横滚角、俯仰角、航向角等三维姿态信息，但它需要初始姿态基准作为积分的初值，而且微小型惯性导航系统的误差随时间急剧增加，一般不能独立工作。

磁传感器能够测量三个正交方向的磁场强度，为了换算为三维姿态信息，还必须首先依赖其它手段，在微小型系统任意运动状态下确定三维磁传感器测量方向与当地水平面之间的坐标转换关系，即必须预先确定横滚角和俯仰角。目前多数磁传感器定姿系统都依赖加速度计或倾角传感器给出静态和匀速直线运动状态下的横滚角和俯仰角，而在其它机动运动状态下磁传感器基本不可用。

可见，任何现有的姿态测量方式均无法准确确定微小型系统在任意运动状态下的三维姿态信息。本发明提出一种适用于任意运动微小型系统的MIMU(Micro Inertial Measure-ment Unit，微惯性测量单元)和GPS辅助MMS(Micro Magnetic Sensors，微磁传感器)定姿系统，利用GPS、MIMU、MMS之间迭代解算关系，不断修正MIMU误差并给出微小型系统任意运动状态下的三维姿态信息，具有广阔的应用前景。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种适用于任意运动微小型系统的定姿系统，该系统利用MIMU和GPS辅助的MMS定姿，为任意运动状态下的微小型系统提供高精度三维姿态信息。

本发明的技术解决方案是：一种适用于任意运动微小型系统的定姿系统，其特征在于：包括微磁传感器MMS模块、微惯性测量单元MIMU模块、GPS模块和Kalman滤波模块，在微小型系统任意运动状态下，MIMU模块根据输出数据通过捷联导航算法分解出微小型系统重力加速度；MMS模块利用所述的重力加速度通过定姿算法实时解算微小型系统的横滚角、俯仰角和航向角三维姿态信息；同时，利用恒虚警CFAR滤波器平滑掉GPS输出速度和加速度信息中的噪声；三维姿态信息一方面送至Kalman滤波模块，与GPS给出的位置和速度信息一道通过信息融合算法间歇校正MIMU误差，另一方面利用三维姿态信息实时更新C_n ^b坐标转换矩阵；坐标转换矩阵C_n ^b将被送给MIMU模块和GPS模块，将水平坐标系的信息转换到载体坐标系下。

本发明的原理是：一般的载体坐标系见图2所示，定义为微小型系统纵轴前进方向为Y轴，向上为Z轴，X轴依据右手定则确定。MIMU、GPS、MMS各自的三维正交测量方向均与载体坐标系三轴重合，见图3所示，GPS位于载体坐标系的原点，MMS、MIMU、三个正交测量轴分别与载体坐标系三轴重合。

MIMU模块由三维正交安装的硅MEMS陀螺和加速度计构成，共六个惯性器件。硅MEMS陀螺测量微小型系统角速度，硅MEMS加速度计测量微小型系统线加速度。在静止状态下硅MEMS加速度计敏感重力加速度在载体坐标系的投影，从而给出载体坐标系与当地水平坐标系的坐标转换矩阵以及横滚角、俯仰角；在其它任意运动状态下，MIMU模块的测量值经过捷联导航算法，利用微小型系统运动线速度、微小型系统运动角速度以及地球自转角速度，实时分解加速度计测量值中微小型系统运动线加速度、哥式加速度、向心加速度、重力加速度等不同成分。利用重力加速度在载体坐标系投影[g_x ^b g_y ^b g_z ^b]^T以及重力加速度在当地水平坐标系投影[0 0 g]^T之间的关系，计算微小型系统的横滚角γ和俯仰角θ，

$\left[\begin{array}{c}{g}_x^b\\ g_y^b\\ g_z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$

MMS模块由安装于三个正交方向的微小型磁传感器组成，测量当前位置三个正交方向的磁场强度，从而给出当前位置磁场矢量的方向和大小。依据横滚角γ和俯仰角θ计算地磁场矢量在当地水平坐标系下的投影：

$H_x^p=H_x^b\cos \mathrm{\γ}+H_z^b\sin \mathrm{\γ}$

$H_y^p=H_x^b\sin \mathrm{\θ}\sin \mathrm{\γ}+H_y^b\cos \mathrm{\θ}-H_z^b\sin \mathrm{\θ}\cos \mathrm{\γ}$

考虑磁偏角λ的情况下计算航向角：

$\mathrm{\φ}=\arctan (H_x^p/H_y^p)-\mathrm{\λ}$

信息融合算法利用当前时刻GPS给出的三维位置及速度信息、MMS给出的三维姿态信息，通过Kalman滤波器校正MIMU的导航误差和惯性器件误差，提高重力加速度在载体坐标系的计算精度。

本发明与现有技术相比的优点在于：(1)MIMU模块结合捷联导航算法不断实时分解出加速度计测量值中重力加速度的成分，并给出重力加速度在载体坐标系的投影，解决了由于加速度计测量值无法分辨运动加速度和重力加速度的问题，从而得到载体坐标系与当地水平坐标系之间的转换关系，即横滚角和俯仰角；(2)采用CFAR滤波器抑制GPS输出数据的噪声，提高了微小型系统位置、速度的测量精度。(3)本发明采用GPS三维位置、速度信息以及MMS给出的三维姿态信息不断校正MIMU的误差，保证了长时间工作情况下MIMU测量精度以及分解重力加速度在载体坐标系投影的精度；(4)MMS模块结合定姿算法能够测量载体坐标系三维方向磁场强度，利用已获得的横滚角和俯仰角，循环迭代计算微小型系统任意运动状态下的三维姿态信息。

附图说明

图1为本发明的适用于任意运动微小型系统的MIMU和GPS辅助MMS定姿系统组成框图；

图2为载体坐标系示意图；

图3为本发明的适用于任意运动微小型系统的MIMU和GPS辅助MMS定姿系统在载体坐标系的安装方法；

图4为本发明的适用于任意运动微小型系统的MIMU和GPS辅助MMS定姿系统工作流程图。

具体实施方式

图1为适用于任意运动微小型系统的MIMU(微惯性测量单元)和GPS辅助MMS(微磁传感器)定姿系统组成框图，包括硬件和软件两部分，硬件部分由MMS模块、MIMU模块、GPS模块组成；软件算法部分包括捷联导航、信息融合以及定姿算法。在微小型系统任意运动状态下，利用MIMU模块的数据和捷联导航算法分解出微小型系统重力加速度，MMS模块利用重力加速度和定姿算法实时解算微小型系统的横滚角、俯仰角和航向角，采用CFAR(恒虚警)滤波器平滑GPS输出噪声，由GPS模块、三维姿态信息以及信息融合算法间歇性的校正MIMU误差，提高重力加速度和姿态解算精度。图4描述了图1中捷联导航算法、定姿算法、信息融合算法基于硬件输出的数据确定姿态的工作流程。

首先，MIMU和GPS辅助的MMS定姿系统在静止状态下开始工作，由MIMU模块中加速度计给出重力加速度在载体坐标系的投影[g_x ^b g_y ^b g_z ^b]^T，已知重力加速度在当地水平坐标系的投影为[0 0 g]^T，

$\left[\begin{array}{c}{g}_x^b\\ g_y^b\\ g_z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$

计算微小型系统的横滚角γ和俯仰角θ，

$\mathrm{\θ}=\arcsin (g_y^b/g)$

$\mathrm{\γ}=-\arcsin (g_x^b/g\cos \mathrm{\θ})$

地磁场在水平坐标系下的投影：

$H_x^p=H_x^b\cos \mathrm{\γ}+H_z^b\sin \mathrm{\γ}$

$H_y^p=H_x^b\sin \mathrm{\θ}\sin \mathrm{\γ}+H_y^b\cos \mathrm{\θ}-H_z^b\sin \mathrm{\θ}\cos \mathrm{\γ}$

由于地磁场的磁力线方向总是指向磁北的，即H_x ^p，H_y ^p的合向量指向磁北，磁航向为

${\mathrm{\φ}}^{\′}=\arctan (H_x^p/H_y^p)$

再用φ′’减去地磁偏角λ，就是数字磁罗盘与真北的夹角即航向角φ。

φ＝φ′-λ

$\mathrm{\φ}=\arctan (H_x^p/H_y^p)-\mathrm{\λ}$

θ，γ，φ即所求的俯仰、横滚和航向角三维姿态信息，并进而刷新载体坐标系b与当地地理坐标系n之间的坐标转换矩阵

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\ψ}\cos \mathrm{\γ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]$

依据静态情况下求出的三维姿态信息分解GPS位置和速度测量值，并进而利用获得的三维位置、三维速度以及航向信息对MIMU进行初始对准，微小型系统进入任意运动状态。

GPS实时输出经度、纬度和高度，经过一阶微分和二阶微分操作得到速度和速度变化率，CFAR滤波器采用滑动平均的方法抑制GPS输出数据的噪声。根据微小型系统运动的机动性设置滑动窗的长度T＝MΔt，采用先进先出的方式确定滑动窗内参与平均的数据，利用当前时刻之前T时段数据经过平均抑制GPS数据的噪声。V^E _t-MΔt～V^E _t，V^N _t-MΔt～V^N _t，V^U _t-MΔt～V^U _t，

表示CFAR平滑之前和之后的东北天坐标系的运动速度和运动加速度。 $V_{\mathrm{en}}^b={\left[\begin{array}{ccc}{\stackrel{\‾}{V}}_t^x& {\stackrel{\‾}{V}}_t^x& {\stackrel{\‾}{V}}_t^x\end{array}\right]}^T$ 和 ${\stackrel{\·}{V}}_{\mathrm{en}}^b={\left[\begin{array}{ccc}{\stackrel{\stackrel{\‾}{\·}}{V}}_t^x& {\stackrel{\stackrel{\‾}{\·}}{V}}_t^x& {\stackrel{\stackrel{\‾}{\·}}{V}}_t^x\end{array}\right]}^T$ 表示载体坐标系的运动速度和加速度矢量。

${\stackrel{\‾}{V}}_t^E=\frac{{V^E}_{t-\mathrm{M\Δt}}+{V^E}_{t-(M-1)\mathrm{\Δt}}+{V^E}_{t-(M-2)\mathrm{\Δt}}+...+{V^E}_t}{\mathrm{M\Δt}}$

${\stackrel{\‾}{V}}_t^N=\frac{{V^N}_{t-\mathrm{M\Δt}}+{V^N}_{t-(M-1)\mathrm{\Δt}}+{V^N}_{t-(M-2)\mathrm{\Δt}}+...+{V^N}_t}{\mathrm{M\Δt}}$

${\stackrel{\‾}{V}}_t^U=\frac{{V^U}_{t-\mathrm{M\Δt}}+{V^U}_{t-(M-1)\mathrm{\Δt}}+{V^U}_{t-(M-2)\mathrm{\Δt}}+...+{V^U}_t}{\mathrm{M\Δt}}$

${\stackrel{\stackrel{\‾}{\·}}{V}}_t^E=\frac{{{\stackrel{\·}{V}}^E}_{t-\mathrm{M\Δt}}+{{\stackrel{\·}{V}}^E}_{t-(M-1)\mathrm{\Δt}}+{{\stackrel{\·}{V}}^E}_{t-(M-2)\mathrm{\Δt}}+...{{\stackrel{\·}{V}}^E}_t}{\mathrm{M\Δt}}$

${\stackrel{\stackrel{\‾}{\·}}{V}}_t^N=\frac{{{\stackrel{\·}{V}}^N}_{t-\mathrm{M\Δt}}+{{\stackrel{\·}{V}}^N}_{t-(M-1)\mathrm{\Δt}}+{{\stackrel{\·}{V}}^N}_{t-(M-2)\mathrm{\Δt}}+...+{{\stackrel{\·}{V}}^N}_t}{\mathrm{M\Δt}}$

${\stackrel{\stackrel{\‾}{\·}}{V}}_t^U=\frac{{{\stackrel{\·}{V}}^U}_{t-\mathrm{M\Δt}}+{{\stackrel{\·}{V}}^U}_{t-(M-1)\mathrm{\Δt}}+{{\stackrel{\·}{V}}^U}_{t-(M-2)\mathrm{\Δt}}+...+{{\stackrel{\·}{V}}^U}_t}{\mathrm{M\Δt}}$

$V_{\mathrm{en}}^b=\left[\begin{array}{c}{\stackrel{\‾}{V}}_t^x\\ {\stackrel{\‾}{V}}_t^y\\ {\stackrel{\‾}{V}}_t^z\end{array}\right]=C_n^b\·\left[\begin{array}{c}{\stackrel{\‾}{V}}_t^E\\ {\stackrel{\‾}{V}}_t^N\\ {\stackrel{\‾}{V}}_t^U\end{array}\right],$ ${\stackrel{\·}{V}}_{\mathrm{en}}^b=\left[\begin{array}{c}{\stackrel{\stackrel{\‾}{\·}}{V}}_t^x\\ {\stackrel{\stackrel{\‾}{\·}}{V}}_t^y\\ {\stackrel{\stackrel{\‾}{\·}}{V}}_t^z\end{array}\right]=C_n^b\·\left[\begin{array}{c}{\stackrel{\stackrel{\‾}{\·}}{V}}_t^E\\ {\stackrel{\stackrel{\‾}{\·}}{V}}_t^N\\ {\stackrel{\stackrel{\‾}{\·}}{V}}_t^U\end{array}\right]$

MIMU模块实时连续输出陀螺、加速度计测量值，并将其输入捷联导航解算，求取载体坐标系下的重力加速度

$g^b=\left[\begin{array}{c}{g}_x^b\\ g_y^b\\ g_z^b\end{array}\right]=\stackrel{\·}{V_{\mathrm{en}}^b}+(2{\mathrm{\ω}}_{\mathrm{ie}}^b+{\mathrm{\ω}}_{\mathrm{en}}^b)\×V_{\mathrm{en}}^b-f^b$

上式各项均为矢量，f^b为加速度计直接测量值(直接测量值在载体坐标系内)；假设L为GPS给出的纬度，当地地理坐标系内地球自转角速度ω_ie ⁿ可分解为

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{iex}}^n& {\mathrm{\ω}}_{\mathrm{iey}}^n& {\mathrm{\ω}}_{\mathrm{iez}}^n\end{array}\right]}^T={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]}^T$

当地地理坐标系内微小型系统运动产生的相对地球坐标系的角速度ω_en ⁿ为

${\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{enx}}^n& {\mathrm{\ω}}_{\mathrm{eny}}^n& {\mathrm{\ω}}_{\mathrm{enz}}^n\end{array}\right]}^T={\left[\begin{array}{ccc}-\frac{{\stackrel{\‾}{V}}_t^N}{R_{\mathrm{yt}}}& \frac{{\stackrel{\‾}{V}}_t^E}{R_{\mathrm{xt}}}& \frac{{\stackrel{\‾}{V}}_t^E}{R_{\mathrm{xt}}}\tan L\end{array}\right]}^T$

V_y ⁿ，V_x ⁿ为利用航向角信息分解GPS速度获得的微小型系统在当地地理坐标系内的北向速度和东向速度信息，R_yt，R_xt分别为地球的子午面半径和卯酉面半径。

得到 $g^b={\left[\begin{array}{ccc}{g}_x^b& g_y^b& g_z^b\end{array}\right]}^T$ 后，即可采用与静止状态相同的算法计算微小型系统横滚角、俯仰角和航向角。

信息融合利用当前获得的三维速度、三维位置和三维姿态信息，采用标准Kalman滤波器校正MIMU的误差。Kalman滤波器状态变量

X(t)＝[φ_x φ_y φ_z δv_x δv_y δv_z δL δλδh ε_x ε_yε_z _x _y _z]^TKalman滤波器观测变量是上文获得的三维速度、三维位置和三维姿态信息与MIMU输出的三维速度、三维位置和三维姿态信息的差值。

Z(t)＝[φ_x φ_y φ_z δv_x δv_y δv_z δL δλδh]^T

根据惯性导航标准误差方程建立Kalman滤波器的状态方程与观测方程，其余计算与标准Kalman滤波器相同。

Claims (6)

1.一种适用于任意运动微小型系统的定姿系统，其特征在于：包括微磁传感器MMS模块、微惯性测量单元MIMU模块、GPS模块和Kalman滤波模块，在微小型系统任意运动状态下，MIMU模块根据输出数据通过捷联导航算法分解出微小型系统重力加速度；MMS模块利用所述的重力加速度通过定姿算法实时解算微小型系统的横滚角、俯仰角和航向角三维姿态信息；同时，利用恒虚警CFAR滤波器平滑掉GPS输出速度和加速度信息中的噪声；三维姿态信息一方面送至Kalman滤波模块，与GPS给出的位置和速度信息一道通过信息融合算法间歇校正MIMU误差，另一方面利用三维姿态信息实时更新C_n ^b坐标转换矩阵；坐标转换矩阵C_n ^b将被送给MIMU模块和GPS模块，将水平坐标系的信息转换到载体坐标系下。

2.根据权利要求1所述的适用于任意运动微小型系统的定姿系统，其特征在于：所述的MMS模块由安装于三个正交方向的微小型磁传感器组成，测量当前位置三个正交方向的地球磁场强度，从而给出当前位置地球磁场矢量的方向和大小。

3.根据权利要求1所述的适用于任意运动微小型系统的定姿系统，其特征在于：所述的MIMU模块中的硅MEMS陀螺测量微小型系统角速度，硅MEMS加速度计测量微小型系统线加速度；在静止状态下，硅MEMS加速度计敏感重力加速度在三维正交方向的投影，给出载体坐标系与当地水平坐标系的坐标转换矩阵以及横滚角、俯仰角。

4.根据权利要求1所述的适用于任意运动微小型系统的定姿系统，其特征在于：所述的CFAR滤波器采用滑动平均的方法抑制GPS输出数据的噪声，根据机动性设置滑动窗的时间长度T＝MΔt，M为滑动个数，Δt为时间间隙，采用先进先出的方式确定滑动窗内参与平均的数据，利用当前时刻之前T时段内数据经过CFAR滤波器抑制GPS数据的噪声。

5.根据权利要求1所述的适用于任意运动微小型系统的定姿系统，其特征在于：所述的捷联导航算法在载体坐标系下建立基本力学编排方程，从而从加速度计测量值中分解出重力加速度在载体坐标系三轴的投影。

6.根据权利要求1所述的适用于任意运动微小型系统的定姿系统，其特征在于：所述的MMS模块利用所述的重力加速度通过定姿算法实时解算微小型系统的横滚角、俯仰角和航向角三维姿态信息的过程如下：利用重力加速度在载体坐标系的投影[g_x ^b g_y ^b g_z ^b]^T以及重力加速度在当地水平坐标系的投影[0 0 g]^T之间的关系，计算微小型系统的横滚角γ和俯仰角θ，

$\left[\begin{array}{c}{g}_x^b\\ g_y^b\\ g_z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$

依据γ和θ计算当前地磁场矢量在水平坐标系下的投影H_x ^p，H_y ^p：

$H_x^p=H_x^b\cos \mathrm{\γ}+H_z^b\sin \mathrm{\γ}$

$H_y^p=H_x^b\sin \mathrm{\θ}\sin \mathrm{\γ}+H_y^b\cos \mathrm{\θ}-H_z^b\sin \mathrm{\θ}\cos \mathrm{\γ}$

考虑磁偏角λ的情况下计算航向角φ：

$\mathrm{\φ}=\arctan (H_x^p/H_y^p)-\mathrm{\λ}.$

Images