CN109345875A - 一种提高船舶自动识别系统测量精度的估计方法 - Google Patents

Abstract

一种提高船舶自动识别系统测量精度的估计方法，属于海上船舶状态测量技术领域。该种方法基于水面船舶三自由度标称运动学模型与动力学模型，分析模型不确定性与测量噪声，建立包含模型不确定性与测量噪声的船舶三自由度受扰模型，进而建立水面船舶三自由度测量方程，基于测量方程设计变结构扩展容积卡尔曼滤波器对船舶运动状态进行估计，利用AIS系统对目标船舶进行跟踪，将估计出的船舶运动状态作为最终目标跟踪状态信息通过通信单元进行发送。本发明的滤波器结合了CKF滤波器与滑模变结构滤波器的优点，既能提供含有高斯条件下测量噪声的二维系统状态信息的精确逼近，又对随机海浪干扰条件下水面船舶三自由度模型不确定性具有良好的鲁棒特性。

Description

一种提高船舶自动识别系统测量精度的估计方法

技术领域

本发明属于海上船舶状态测量技术领域，具体涉及一种提高船舶自动识别系统测量精度的估计方法。

背景技术

随着社会经济的快速发展，不断增长的船舶数量和交通密度严重影响着水域交通安全及海洋环境污染。频繁发生的海损事故，将会造成生命与财产的重大损失，而且对邻近水域的海洋生态环境会造成严重的破坏。为了确保海上航行安全，船与船、船与岸之间的实时数据交换以及目标信息识别等功能变得越来越重要。船舶自动识别系统(AutomaticIdentification System，AIS)正是针对上述问题而发展起来的一种用于岸一船、船一岸以及船一船之间通信的新型助航系统。AIS设备能将本船的动态信息如位置、速度、航向等和静态信息如海上移动业务标识码、船名、呼号等，经信息处理器处理后在VHF信道向外播发，本船周围船舶只要安装了AIS设备就可以自动接收这些船舶信息并显示；同时，本船也可将其他船的识别信息和本船信息一起存储并向范围内其他目标船发送。然而，由于船舶模型不确定性、随机环境干扰与高频测量噪声等因素的存在，传统的AIS系统得出的目标船舶运动状态信息存在较大的测量偏差，严重影响了AIS系统对目标船舶的判别，这种测量偏差的存在，对船舶海上航行的安全性产生了巨大的威胁。

发明内容

本发明的目的是为了解决水面船舶自动识别系统提供的船舶运动状态数据存在较大测量误差的问题，提供一种提高船舶自动识别系统测量精度的估计方法，该种方法基于水面船舶三自由度标称运动学模型与动力学模型，分析水面船舶三自由度模型中的模型不确定性与测量噪声，建立包含模型不确定性与测量噪声的船舶三自由度受扰模型，进而建立水面船舶三自由度测量方程，基于测量方程设计变结构扩展容积卡尔曼滤波器对船舶运动状态进行估计，利用AIS系统对目标船舶进行跟踪，将估计出的船舶运动状态作为最终目标跟踪状态信息通过通信单元进行发送。

为实现上述目的，本发明采取的技术方案如下：

一种提高船舶自动识别系统测量精度的估计方法，所述的方法具体步骤如下：

步骤一：建立水面船舶三自由度标称运动学模型与动力学模型为：

其中，η＝[x,y,ψ]^T，ν＝[u,v,r]^T，x为北向位置，y为东向位置，ψ为船舶艏向角，u为纵荡速度，v为横荡速度，r为艏摇角速度，J(η)为坐标转换矩阵，τ_c为控制力和力矩的矩阵；

M₀为船舶质量与惯性矩阵，其详细定义为：

其中，m为船体质量，I_Z为Z轴转动惯量，x_G为船体重心在船体坐标系下x轴的坐标， 均是指相应自由度下的附加质量和附加静矩；

C₀(ν)为科里奥利与向心力矩阵，其详细定义为：

是指相应自由度的附加质量和附加静矩；

D₀(ν)为水动力阻尼矩阵，其详细定义为：

其中，X_u、Y_v、Y_r、N_v、N_r分别为相应自由度的线性水动力阻尼；

步骤二：分析水面船舶三自由度标称动力学模型中的不确定性与测量噪声，建立包含模型不确定性与测量噪声的船舶三自由度受扰模型；

基于式(10)给出模型不确定性分析如下，设置以下变量：

M＝M₀+M_Δ (11)

C(ν)＝C₀(ν)+C_Δ(ν) (12)

D(ν)＝D₀(ν)+D_Δ(ν) (13)

其中，带有下标0的表示矩阵中标称模型部分，带有下标Δ的表示模型参数摄动部分，则含有模型不确定性的船舶三自由度受扰模型表示成以下形式：

其中，式(10)为带下标0的标称模型，式(14)为添加了模型不确定性的模型，含义不同；

由一阶波浪诱导产生的水面船舶三自由度高频运动模型由下式所示

其中，ξ为等效状态变量，A_w为系统矩阵，E_w为随机噪声矩阵，w₁为零均值高斯白噪声向量，η_w＝C_wξ代表一阶线性波浪诱导作用下的状态高频输出，C_w为输出矩阵；

将公式(15)所描述的模型看作是一种附带非线性阻尼的二阶振荡环节，如下式所示：

其中，K_ωi为波浪强度度量值，s是为传递函数中拉氏变换的算子，阻尼比ζ_i的取值范围为0.05～0.2，ω_0i为波浪P-M谱中的主导频率；

利用一阶高斯—马尔科夫过程模拟由海上环境因素引起的缓慢变化的环境扰动作用力，该慢变环境扰动模型的描述形式为：

其中，T_b为二维对角阵，E_b为随机噪声矩阵，w₂为零均值高斯白噪声向量；

考虑到水面船舶导航与测量系统能够提供的是带有测量噪声的系统输出值，因此，测量方程由下式得出：

y＝η+η_w+v (18)

其中，v为零均值高斯白噪声向量；

综上，得到包含环境干扰与随机噪声特性的完整系统模型为：

为了便于步骤中变结构扩展容积卡尔曼滤波器的设计，提取式(19)中的第二与第四子方程离散化写成如下的一体化标准状态空间描述形式：

其中，x_k＝[η_k,υ_k]^T，η_k和υ_k即表征离散后的第k时刻状态η和状态υ，下标k表征当前的k时刻，下标k-1表示k-1时刻，

u_k-1＝τ_ck-1，w_k-1由于传感器高频噪声和随机海浪干扰产生的随机系统噪声，z_k为k时刻系统的观测输出，h(·)＝η，v_k为k时刻传感器测量噪声；

步骤三：基于测量方程设计变结构扩展容积卡尔曼滤波器对船舶运动状态进行估计；首先容积卡尔曼滤波CKF选取2n个Cubature点及同等权值计算高斯权重积分，Cubature点形式如下：

使用Cubature点集得出CKF滤波算法，而后进行状态扩维，在每个滤波周期内进行时间更新和测量更新，n为自然数、e_i为状态矩阵的第i个列元素；

对于基础型扩展CKF滤波器设计步骤包含以下几部分：

1.时间更新：

a.假设k-1时刻后验密度函数已知，通过Cholesky分解误差协方差矩阵P_k-1|k-1：

其中，N(·,·)高斯分布，为k-1时刻系统状态[η,υ]^T的估计值，S_k-1|k-1为通过Cholesky分解获取得到的正定对称阵，为S_k-1|k-1的转置；

b.针对系统状态进行状态扩维(j＝1,2,...N,N＝2(n+n+m))：

n为状态的维数，m为噪声的维数，j是要进行取Cubature点的数量

其中，为扩维后k-1时刻系统状态[η,υ]^T的估计值，Q和R都为正定对称矩阵，即表示扩展之后的S_k-1|k-1矩；

c.计算Cubature点(j＝1,2,...N,N＝2(n+n+m))：

其中，X_j,k-1|k-1为Cubature点；

通过状态方程传播的Cubature点

其中，r表示采样基准，x表示采样域，表示与系统状态维数相对应的Cubature点，表示与状态噪声相对应的Cubature点，u_k-1为k-1时刻的控制输入；

d.估计k时刻的状态预测值如下：

其中，是在k时刻的预测值；

估计k时刻的状态误差协方差预测值P_k|k-1如下：

2.测量更新：

a.通过观测方程传播的Cubature点Z_i,k|k-1如下：

其中，v表示基准采样点，x表示采样域，为与系统状态维数相对应的Cubature点，表示与系统观测噪声相对应的Cubature点；

估计k时刻的观测预测值

Z_j,k|k-1为求平均数用的原始元素；

b.估计自相关协方差阵P_zz,k|k-1：

估计互相关协方差阵P_xz,k|k-1：

X_j,k|k-1为Cubature点的异步预报估计；ω_j＝1/2n,j＝1,2,...,n；

估计卡尔曼增益W_k：

c.k时刻的状态估计值

k时刻状态误差协方差估计值P_k|k：

P_k|k＝P_k|k-1-W_kP_zz,k|k-1W_k ^T (35)

对于滑模变结构滤波器，

1.k时刻状态预测值：

其中，

2.估计k时刻的状态误差协方差预测值：

其中， 即对函数f求取偏导数；Q_k-1代表k-1时刻的矩阵Q，Q为正定对称阵；u_k就是k时刻的控制输入u；

3.则k时刻的观测预测值为：

其中，h(·)＝η；

设观测值z的估计误差变量e_z,k|k-1为：

4.滤波器增益K_k表示为：

其中，○为Schur乘法算子，算子[·]⁺代表矩阵的广义逆，γ为标量，代表滤波器的收敛率增益参数，e_z,k-1|k-1为k-1时刻系统观测误差变量，z_k-1为k-1时刻的系统观测值，为k-1时刻系统输出的估计值；

滤波器增益的饱和函数定义为：

其中，为由滑动平面边界向量ψ导出的对角阵，带下标i的表征矩阵里面的某一个元素；不带i的表示整个矩阵；

ψ_m代表矩阵里面第m行第m列的那个元素中的分母，是滑动平面边界向量中的元素；

k时刻的状态估计值为：

其中，K_k为滤波器增益参数，e_z,k|k-1为估计误差；

k时刻的状态误差协方差估计值：

P_k|k＝(I-K_kH_k-1)P_k|k-1(I-K_kH_k-1)^T+K_kR_kK_k ^T (49)

其中，矩阵R_k为待选择的正定对称阵；I为单位阵；P_k|k-1代表正定对称阵P在k-1时刻的异步预报估计；

k时刻的观测估计值和观测误差估计值依次为：

定义滑动平面边界层矩阵ψ，该矩阵包含了不同滑动平面边界层之间的关系；

为了解决基于ψ的滑模面边界层随时间变化的问题，将式(49)和k时刻的状态误差协方差估计值方程联立可得：

其中，trace代表矩阵的迹；上式解定义为如下形式：

其中，S_k和A分别定义为：

A＝(|e_z,k|k-1|+γ|e_z,k-1|k-1|) (55)

由可知，代表由A组成的对角阵，利用此对角阵计算得出的滑动平面边界方程，将扩展Cubature卡尔曼滤波器与滑模变结构滤波器联合进行目标船舶三自由度状态估计；

步骤四：基于步骤一至三中对AIS系统中目标水面船舶三自由度运动状态的最优估计，对目标船舶运动状态进行持续目标跟踪，将基于容积卡尔曼滤波器估计出的船舶运动状态作为最终目标跟踪状态信息通过通信单元进行发送，从而基于此判断本船是否通过危险区域。

本发明相对于现有技术的有益效果是：本发明采用一种基于扩展CKF的变结构滤波器作为船舶自动识别系统对目标船舶运动状态进行最优估计。该滤波器结合了CKF滤波器与滑模变结构滤波器的优点，既能提供含有高斯条件下测量噪声的二维系统状态信息的精确逼近，又对随机海浪干扰条件下水面船舶三自由度模型不确定性具有良好的鲁棒特性；由于利用状态估计策略对AIS系统输出数据进行最优状态估计，因此提高了AIS系统对目标船舶运动状态的检测精度。

附图说明

图1为基于变结构扩展CKF滤波的船舶自动识别系统工作流程图；

图2为变结构滤波器原理示意图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明，但并不局限于此，凡是对本发明技术方案进行修正或等同替换，而不脱离本发明技术方案的精神范围，均应涵盖在本发明的保护范围之中。本发明中符号上带一个点的表示一阶导数，带两个点表示二阶导数。

具体实施方式一：本实施方式记载的是一种提高船舶自动识别系统测量精度的估计方法，所述的方法具体步骤如下：

步骤一：建立水面船舶三自由度标称运动学模型与动力学模型为：

其中，η＝[x,y,ψ]^T，ν＝[u,v,r]^T，x为北向位置，y为东向位置，ψ为船舶艏向角，u为纵荡速度，v为横荡速度，r为艏摇角速度，J(η)为坐标转换矩阵，τ_c为控制力和力矩的矩阵；

M₀为船舶质量与惯性矩阵，其详细定义为：

其中，m为船体质量，I_Z为Z轴转动惯量，x_G为船体重心在船体坐标系下x轴的坐标， 均是指相应自由度下的附加质量和附加静矩；

C₀(ν)为科里奥利与向心力矩阵，其详细定义为：

是指相应自由度的附加质量和附加静矩；

D₀(ν)为水动力阻尼矩阵，其详细定义为：

其中，X_u、Y_v、Y_r、N_v、N_r分别为相应自由度的线性水动力阻尼；

步骤二：分析水面船舶三自由度标称动力学模型中的不确定性与测量噪声，建立包含模型不确定性与测量噪声的船舶三自由度受扰模型；

由于实际模型中存在诸多未建模动态和模型不确定性，因此需要基于式(10)给出模型不确定性分析如下，设置以下变量：

M＝M₀+M_Δ (11)

C(ν)＝C₀(ν)+C_Δ(ν) (12)

D(ν)＝D₀(ν)+D_Δ(ν) (13)

其中，带有下标0的表示矩阵中标称模型部分，带有下标Δ的表示模型参数摄动部分，则含有模型不确定性的船舶三自由度受扰模型表示成以下形式：

其中，式(10)为带下标0的标称模型，式(14)为添加了模型不确定性的模型，含义不同；

由一阶波浪诱导产生的水面船舶三自由度高频运动模型由下式所示

其中，ξ为等效状态变量，A_w为系统矩阵，E_w为随机噪声矩阵，w₁为零均值高斯白噪声向量，η_w＝C_wξ代表一阶线性波浪诱导作用下的状态高频输出，C_w为输出矩阵，其数值的大小代表不同的海情状态；

将公式(15)所描述的模型看作是一种附带非线性阻尼的二阶振荡环节，如下式所示：

其中，K_ωi为波浪强度度量值，K_ωi越大，则波浪强度越大，s是为传递函数中拉氏变换的算子，阻尼比ζ_i的取值范围为0.05～0.2，ω_0i为波浪P-M谱中的主导频率，与有义波高值有关；

利用一阶高斯—马尔科夫过程模拟由风浪流等海上环境因素引起的缓慢变化的环境扰动作用力，诸如二阶波浪漂移力、海流作用力以及海风等作用力。该慢变环境扰动模型的描述形式为：

其中，T_b为二维对角阵，表征环境干扰力矩的时间常数，E_b为随机噪声矩阵，w₂为零均值高斯白噪声向量；

考虑到水面船舶导航与测量系统能够提供的是带有测量噪声的系统输出值，因此，测量方程由下式得出：

y＝η+η_w+v (18)

其中，v为零均值高斯白噪声向量，表征传感器系统测量噪声；

综上，得到包含环境干扰与随机噪声特性的完整系统模型为：

为了便于步骤中变结构扩展容积卡尔曼滤波器的设计，可以提取式(19)中的第二与第四子方程离散化写成如下的一体化标准状态空间描述形式：

其中，x_k＝[η_k,υ_k]^T，η_k和υ_k即表征离散后的第k时刻状态η和状态υ，下标k表征当前的k时刻，下标k-1表示k-1时刻，u_k-1＝τ_ck-1，w_k-1由于传感器高频噪声和随机海浪干扰产生的随机系统噪声，z_k为k时刻系统的观测输出，h(·)＝η，v_k为k时刻传感器测量噪声；

步骤三：基于测量方程设计变结构扩展容积卡尔曼滤波器对船舶运动状态进行估计；首先容积卡尔曼滤波CKF选取2n个Cubature点及同等权值计算高斯权重积分，Cubature点形式如下：

使用Cubature点集得出CKF滤波算法，而后进行状态扩维，在每个滤波周期内进行时间更新和测量更新，n为自然数、e_i为状态矩阵的第i个列元素；

对于基础型扩展CKF滤波器设计步骤包含以下几部分：

1.时间更新：

a.假设k-1时刻后验密度函数已知，通过Cholesky分解误差协方差矩阵P_k-1|k-1：

其中，N(·,·)高斯分布，为k-1时刻系统状态[η,υ]^T的估计值，S_k-1|k-1为通过Cholesky分解获取得到的正定对称阵，为S_k-1|k-1的转置；

b.针对系统状态进行状态扩维(j＝1,2,...N,N＝2(n+n+m))：

n为状态的维数，m为噪声的维数，j是要进行取Cubature点的数量

其中，为扩维后k-1时刻系统状态[η,υ]^T的估计值，Q和R都为正定对称矩阵，即表示扩展之后的S_k-1|k-1矩；

c.计算Cubature点(j＝1,2,...N,N＝2(n+n+m))：

其中，X_j,k-1|k-1为Cubature点；

通过状态方程传播的Cubature点

其中，r表示采样基准，x表示采样域，表示与系统状态维数相对应的Cubature点，表示与状态噪声相对应的Cubature点，u_k-1为k-1时刻的控制输入；

d.估计k时刻的状态预测值如下：

其中，是在k时刻的预测值；

估计k时刻的状态误差协方差预测值P_k|k-1如下：

2.测量更新：

a.通过观测方程传播的Cubature点Z_i,k|k-1如下：

其中，v表示基准采样点，x表示采样域，为与系统状态维数相对应的Cubature点，表示与系统观测噪声相对应的Cubature点；

估计k时刻的观测预测值

Z_j,k|k-1为求平均数用的原始元素；

b.估计自相关协方差阵P_zz,k|k-1：

估计互相关协方差阵P_xz,k|k-1：

X_j,k|k-1为Cubature点的异步预报估计；ω_j＝1/2n,j＝1,2,...,n；

估计卡尔曼增益W_k：

c.k时刻的状态估计值

k时刻状态误差协方差估计值P_k|k：

P_k|k＝P_k|k-1-W_kP_zz,k|k-1W_k ^T (35)

水面船舶在整个工作域范围内是一个典型的具有模型参数不确定性的强非线性动力系统，而滑模变结构滤波器对于该类系统表现出很好的稳定性和鲁棒性。滑模变结构滤波器的基本概念如图2所示。

对于滑模变结构滤波器，

1.k时刻状态预测值：

其中，

2.估计k时刻的状态误差协方差预测值：

其中， 即对函数f求取偏导数；Q_k-1代表k-1时刻的矩阵Q，Q为正定对称阵；u_k就是k时刻的控制输入u；

3.则k时刻的观测预测值为：

其中，h(·)＝η；

设观测值z的估计误差变量e_z,k|k-1为：

4.滤波器增益K_k表示为：

其中，○为Schur乘法算子，算子[·]⁺代表矩阵的广义逆，γ为标量，代表滤波器的收敛率增益参数，e_z,k-1|k-1为k-1时刻系统观测误差变量，z_k-1为k-1时刻的系统观测值，为k-1时刻系统输出的估计值；

滤波器增益的饱和函数定义为：

其中，为由滑动平面边界向量ψ导出的对角阵，带下标i的表征矩阵里面的某一个元素；不带i的表示整个矩阵；

ψ_m代表矩阵里面第m行第m列的那个元素中的分母，是滑动平面边界向量中的元素；

k时刻的状态估计值为：

其中，K_k为滤波器增益参数，e_z,k|k-1为估计误差；

k时刻的状态误差协方差估计值：

P_k|k＝(I-K_kH_k-1)P_k|k-1(I-K_kH_k-1)^T+K_kR_kK_k ^T (49)

其中，矩阵R_k为待选择的正定对称阵；I为单位阵；P_k|k-1代表正定对称阵P在k-1时刻的异步预报估计；

k时刻的观测估计值和观测误差估计值依次为：

为了得到一个能够产生更精确估计值的滑动平面的边界方程，定义了如下的滑动平面边界层矩阵ψ，该矩阵包含了不同滑动平面边界层之间的关系；

为了解决基于ψ的滑模面边界层随时间变化的问题，将式(49)和k时刻的状态误差协方差估计值方程联立可得：

其中，trace代表矩阵的迹；上式解定义为如下形式：

其中，S_k和A分别定义为：

A＝(|e_z,k|k-1|+γ|e_z,k-1|k-1|) (55)

式(54)、(55)中相关参数定义可参见前文定义；

由可知，代表由A组成的对角阵，利用此对角阵计算得出的滑动平面边界方程，可以将扩展Cubature卡尔曼滤波器与滑模变结构滤波器联合进行目标船舶三自由度状态估计；

步骤四：基于步骤一至三中对AIS系统中目标水面船舶三自由度运动状态的最优估计，对目标船舶运动状态进行持续目标跟踪，将基于容积卡尔曼滤波器估计出的船舶运动状态作为最终目标跟踪状态信息通过通信单元进行发送，从而基于此判断本船是否通过危险区域。

Claims (1)

1.一种提高船舶自动识别系统测量精度的估计方法，其特征在于：所述的方法具体步骤如下：

步骤一：建立水面船舶三自由度标称运动学模型与动力学模型为：

其中，η＝[x,y,ψ]^T，ν＝[u,v,r]^T，x为北向位置，y为东向位置，ψ为船舶艏向角，u为纵荡速度，v为横荡速度，r为艏摇角速度，J(η)为坐标转换矩阵，τ_c为控制力和力矩的矩阵；

M₀为船舶质量与惯性矩阵，其详细定义为：

其中，m为船体质量，I_Z为Z轴转动惯量，x_G为船体重心在船体坐标系下x轴的坐标， 均是指相应自由度下的附加质量和附加静矩；

C₀(ν)为科里奥利与向心力矩阵，其详细定义为：

是指相应自由度的附加质量和附加静矩；

D₀(ν)为水动力阻尼矩阵，其详细定义为：

其中，X_u、Y_v、Y_r、N_v、N_r分别为相应自由度的线性水动力阻尼；

步骤二：分析水面船舶三自由度标称动力学模型中的不确定性与测量噪声，建立包含模型不确定性与测量噪声的船舶三自由度受扰模型；

基于式(10)给出模型不确定性分析如下，设置以下变量：

M＝M₀+M_Δ (11)

C(ν)＝C₀(ν)+C_Δ(ν) (12)

D(ν)＝D₀(v)+D_Δ(v) (13)

其中，带有下标0的表示矩阵中标称模型部分，带有下标Δ的表示模型参数摄动部分，则含有模型不确定性的船舶三自由度受扰模型表示成以下形式：

其中，式(10)为带下标0的标称模型，式(14)为添加了模型不确定性的模型，含义不同；

由一阶波浪诱导产生的水面船舶三自由度高频运动模型由下式所示

其中，ξ为等效状态变量，A_w为系统矩阵，E_w为随机噪声矩阵，w₁为零均值高斯白噪声向量，η_w＝C_wξ代表一阶线性波浪诱导作用下的状态高频输出，C_w为输出矩阵；

将公式(15)所描述的模型看作是一种附带非线性阻尼的二阶振荡环节，如下式所示：

其中，K_ωi为波浪强度度量值，s是为传递函数中拉氏变换的算子，阻尼比ζ_i的取值范围为0.05～0.2，ω_0i为波浪P-M谱中的主导频率；

利用一阶高斯—马尔科夫过程模拟由海上环境因素引起的缓慢变化的环境扰动作用力，该慢变环境扰动模型的描述形式为：

其中，T_b为二维对角阵，E_b为随机噪声矩阵，w₂为零均值高斯白噪声向量；

考虑到水面船舶导航与测量系统能够提供的是带有测量噪声的系统输出值，因此，测量方程由下式得出：

y＝η+η_w+v (18)

其中，v为零均值高斯白噪声向量；

综上，得到包含环境干扰与随机噪声特性的完整系统模型为：

为了便于步骤中变结构扩展容积卡尔曼滤波器的设计，提取式(19)中的第二与第四子方程离散化写成如下的一体化标准状态空间描述形式：

其中，x_k＝[η_k,υ_k]^T，η_k和υ_k即表征离散后的第k时刻状态η和状态υ，下标k表征当前的k时刻，下标k-1表示k-1时刻，u_k-1＝τ_ck-1，w_k-1由于传感器高频噪声和随机海浪干扰产生的随机系统噪声，z_k为k时刻系统的观测输出，h(·)＝η，v_k为k时刻传感器测量噪声；

步骤三：基于测量方程设计变结构扩展容积卡尔曼滤波器对船舶运动状态进行估计；首先容积卡尔曼滤波CKF选取2n个Cubature点及同等权值计算高斯权重积分，Cubature点形式如下：

使用Cubature点集得出CKF滤波算法，而后进行状态扩维，在每个滤波周期内进行时间更新和测量更新，n为自然数、e_i为状态矩阵的第i个列元素；

对于基础型扩展CKF滤波器设计步骤包含以下几部分：

1.时间更新：

a.假设k-1时刻后验密度函数已知，通过Cholesky分解误差协方差矩阵P_k-1|k-1：

其中，N(·,·)高斯分布，为k-1时刻系统状态[η,υ]^T的估计值，S_k-1|k-1为通过Cholesky分解获取得到的正定对称阵，为S_k-1|k-1的转置；

b.针对系统状态进行状态扩维(j＝1,2,...N,N＝2(n+n+m))：

n为状态的维数，m为噪声的维数，j是要进行取Cubature点的数量

其中，为扩维后k-1时刻系统状态[η,υ]^T的估计值，Q和R都为正定对称矩阵，即表示扩展之后的S_k-1|k-1矩；

c.计算Cubature点(j＝1,2,...N,N＝2(n+n+m))：

其中，X_j,k-1|k-1为Cubature点；

通过状态方程传播的Cubature点

其中，r表示采样基准，x表示采样域，表示与系统状态维数相对应的Cubature点，表示与状态噪声相对应的Cubature点，u_k-1为k-1时刻的控制输入；

d.估计k时刻的状态预测值如下：

其中，是在k时刻的预测值；

估计k时刻的状态误差协方差预测值P_k|k-1如下：

2.测量更新：

a.通过观测方程传播的Cubature点Z_i,k|k-1如下：

其中，v表示基准采样点，x表示采样域，为与系统状态维数相对应的Cubature点，表示与系统观测噪声相对应的Cubature点；

估计k时刻的观测预测值

Z_j,k|k-1为求平均数用的原始元素；

b.估计自相关协方差阵P_zz,k|k-1：

估计互相关协方差阵P_xz,k|k-1：

X_j,k|k-1为Cubature点的异步预报估计；ω_j＝1/2n,j＝1,2,...,n；

估计卡尔曼增益W_k：

c.k时刻的状态估计值

k时刻状态误差协方差估计值P_k|k：

P_k|k＝P_k|k-1-W_kP_zz,k|k-1W_k ^T (35)

对于滑模变结构滤波器，

1.k时刻状态预测值：

其中，

2.估计k时刻的状态误差协方差预测值：

其中， 即对函数f求取偏导数；Q_k-1代表k-1时刻的矩阵Q，Q为正定对称阵；u_k就是k时刻的控制输入u；

3.则k时刻的观测预测值为：

其中，h(·)＝η；

设观测值z的估计误差变量e_z,k|k-1为：

4.滤波器增益K_k表示为：

其中，○为Schur乘法算子，算子[·]⁺代表矩阵的广义逆，γ为标量，代表滤波器的收敛率增益参数，e_z,k-1|k-1为k-1时刻系统观测误差变量，z_k-1为k-1时刻的系统观测值，为k-1时刻系统输出的估计值；

滤波器增益的饱和函数定义为：

其中，为由滑动平面边界向量ψ导出的对角阵，带下标i的表征矩阵里面的某一个元素；不带i的表示整个矩阵；

ψ_m代表矩阵里面第m行第m列的那个元素中的分母，是滑动平面边界向量中的元素；

k时刻的状态估计值为：

其中，K_k为滤波器增益参数，e_z,k|k-1为估计误差；

k时刻的状态误差协方差估计值：

其中，矩阵R_k为待选择的正定对称阵；I为单位阵；P_k|k-1代表正定对称阵P在k-1时刻的异步预报估计；

k时刻的观测估计值和观测误差估计值依次为：

定义滑动平面边界层矩阵ψ，该矩阵包含了不同滑动平面边界层之间的关系；

为了解决基于ψ的滑模面边界层随时间变化的问题，将式(49)和k时刻的状态误差协方差估计值方程联立可得：

其中，trace代表矩阵的迹；上式解定义为如下形式：

其中，S_k和A分别定义为：

A＝(|e_z,k|k-1|+γ|e_z,k-1|k-1|) (55)

由可知，代表由A组成的对角阵，利用此对角阵计算得出的滑动平面边界方程，将扩展Cubature卡尔曼滤波器与滑模变结构滤波器联合进行目标船舶三自由度状态估计；

步骤四：基于步骤一至三中对AIS系统中目标水面船舶三自由度运动状态的最优估计，对目标船舶运动状态进行持续目标跟踪，将基于容积卡尔曼滤波器估计出的船舶运动状态作为最终目标跟踪状态信息通过通信单元进行发送，从而基于此判断本船是否通过危险区域。