[发明专利]一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法

摘要

本发明提出了一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法，属于极区导航技术领域，通过建立横坐标系下卡尔曼滤波误差模型，估计水平姿态误差角，利用反馈校正不断校正水平姿态误差角，从而抑制姿态、速度和位置误差中的舒勒振荡，实现卡尔曼滤波外水平阻尼。根据外速度有效性判据判断外速度信息的可靠性；并根据判断结果确定当前捷联惯导系统的运行状态，直到外速度信息有效时，系统从无阻尼切换至阻尼状态；最后待卡尔曼滤波稳定后，将估计的水平姿态误差角不断地进行反馈校正，从而抑制姿态、位置和速度误差中的舒勒振荡，实现极区卡尔曼滤波外水平阻尼。本发明不仅能实现极区外水平阻尼，还能抑制阻尼切换过程产生的超调误差。

主权项

1.一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法，其特征在于，通过以下步骤实现：(1)建立横坐标系捷联惯导力学编排，获得横坐标系下的运动学方程：其中，表示载体坐标系b到横导航坐标系的方向余弦矩阵；为载体的对地速度在横导航系下的投影；为载体在横坐标系下的位置矢量；为地球自转角速度在横导航系下的投影；为横导航系相对于横地球坐标系的旋转角速率在横导航系下的投影；g为当地重力加速度大小；R为地球半径；R_c为位置方向余弦矩阵；为捷联惯导系统陀螺仪输出；f^b为加速度计输出；(2)根据运动学方程，针对舰船使用的横坐标系捷联惯导系统，建立横坐标系捷联惯导系统的状态方程和量测方程：Z＝HX+η其中A为状态转移矩阵；状态变量为为系统噪声；B为系统噪声驱动阵；H为观测阵；η为量测噪声；(3)建立多普勒计程仪速度信息有效性判据，判断外速度信息有效性是否发生改变；其中，多普勒计程仪为Doppler velocity log，DVL；(4)若步骤(3)判断外速度信息从有效变为无效，执行步骤(5)；若步骤(3)判断外速度信息从无效变为有效，执行步骤(6)；若步骤(3)判断外速度信息有效性状态不变，则系统继续执行当前状态；(5)系统从阻尼切换至无阻尼状态，直接执行纯惯导下的姿态、速度和位置的解算；(6)系统从无阻尼切换至阻尼状态，通过运行卡尔曼滤波器估计出水平姿态误差角，待滤波器稳定后，将估计的水平姿态误差角进行反馈校正，从而实现外水平阻尼。