[0043] 本发明包括以下步骤：
[0044] 步骤1：基于传统UKF的非线性状态估计：
[0045] 无迹卡尔曼滤波器是传统KF的一个改进，它的基本原理与传统的KF相同。一般KF算法被分为两个阶段，第一个阶段为预测，因为它利用系统演化预测模型根据前一个状态生成一个先验的系统状态估计。第二个阶段为更新，通过修正先前的先验估计，将系统中的测量值考虑在内，从而产生后验状态估计。这两阶段过程始于一个初始估计状态，在一个循环中重复递归直到滤波结束，算法流程如图1所示。
[0046] 将USV模型化为状态空间形式，用离散系统状态向量x来描述USV运动的非线性系统：
[0047] x(k)＝f(x(k‑1),w(k‑1)) (10)
[0048] z(k)＝Hx(k)+v(k) (11)
[0049] 其中f()为非线性状态转移矩阵，w(k)是过程噪声，H为测量矩阵，v(k)为测量噪声，k为当前时刻，k‑1为上一时刻。假设它们都是满足正态概率分布p(w)～N(0,Q)和p(v)～N(0,R)的高斯白噪声。
[0050] 选取横向位置px，纵向位置py，艏摇角ψ，前进速度u，横移速度v，艏摇角速度r，作为状态向量：
[0051] x＝[px,py,ψ,u,v,r]T (12)
[0052] 输出的测量向量为：
[0053]
[0054] 通过USV动力学方程得到状态空间型的非线性USV离散系统：
[0055]
[0056] 其中m为无人艇质量，Xu、Yv、Nr为流体动力的速度导数， 为流体动力的加速度导数，Iz为惯性矩，XP1、XP2为两个螺旋桨的转速，dp为两个螺旋桨的横向距离，为前进速度的导数，为横移速度的导数。
[0057] 根据GPS、电子罗盘、IMU和状态向量的转换关系得到系数矩阵H：
[0058]
[0059] 其中前两行由GPS获得位置信息，第三行由电子罗盘获得航向角信息，最后3行由IMU中的加速度和陀螺仪获得速度信息，T为采样间隔。
[0060] 然后运行传统UKF算法计算出 Kk, 分别为状态后验估计，测量误差协方差矩阵，卡尔曼系数和误差协方差矩阵的后验估计。
[0061] 步骤2：Q自适应调整：
[0062] 根据当前理论估计值和最后一次数据的加权组合自适应地调整Q和R。由于UKF在正常情况下工作良好，因此采用故障检测机制来判断是否需要修正噪声协方差矩阵。该算法采用的故障检测机制使用以下统计功能来检测故障：
[0063]2
[0064] 其中 是一个自由度为s的χ分布，s为μk的维度。创新序列μk定义为为假设想检测一个可靠性等级为1‑σ的故障，其中σ是选择的参数，然后有一个阈值通过 的 分布决定
[0065]
[0066] 因此，利用预先选择的σ值，从式中定义一个对应的故障检测阈值 使得系统故障可以检测到1‑σ的可靠性等级。
[0067] 创新序列 作为新量测的结果，是滤波器自适应最相关的信息，可以用来估计噪声协方差，过程噪声可以表示为
[0068] wk‑1＝xk‑f(xk‑1) (15)
[0069] 过程噪声协方差矩阵Qk‑1的估计可以估计为:
[0070]
[0071] 其中cov()为协方差，E()为期望，Kk为卡尔曼增益。
[0072] 为了实现上述方程， 通常通过使用窗口方法平均 随时间的变化来近似。本发明没有采用移动窗口方法，而是利用加权因子λ来自适应地调整Q，以平衡最后的噪声协方差值和当前估计。加权因子λ设置为下限λ0∈(0,1)，以确保更新强度，并且如果λ超过预设阈值 将随着 的升高而增加。因此系统处理噪声协方差矩阵更新为：
[0073]
[0074]
[0075] 其中Kk为卡尔曼增益， 是一个自由度为s的χ2分布，s为μk的维度。创新序列μk定义为 zk为传感器测量向量， 为测量估计向量。 为对应的故障检测阈值。a(a>0)是依赖于实际环境的调整参数，较大的a意味着使用默认λ0的概率较高。λ0和a参数的调整决定了协方差更新对新数据的敏感程度。
[0076] 步骤3：R自适应调整：
[0077] 基于时间步长k的测量噪声协方差矩阵R也可以采用基于创新的方法进行估计，具体如下:
[0078]
[0079] 其中 为了得到正定矩阵 本发明采用了基于残差的方法。可以导出时间步k处的测量噪声为vk＝zk‑h(xk)。然后，在时间步k处的剩余向量由[0080]
[0081] 此外，基于残差向量k的测量噪声协方差的估计如下
[0082]
[0083] 其中
[0084]
[0085]
[0086]
[0087] 和对Q自适应调整类似，使用δ设下限δ0∈(0,1)的加权因子更新Rk[0088]
[0089]
[0090] 其中b(b>0)也是依赖于实际环境的调整参数，b和δ0的选择与a和λ0的选择相同。
[0091] 步骤4：修正估计：
[0092] 一旦过程噪声协方差矩阵Q和测量噪声协方差R被更新，当前状态估计可以用新的R、Q、估计的状态x和估计的误差协方差P来修正，以获得更精确的状态估计。修正过程如下所述：
[0093] 1.计算新的预测误差协方差矩阵Pk和互协方差矩阵Pxz，将获得的状态x视为时间步k的预测状态：
[0094]
[0095]
[0096] 2.评估更新后的创新协方差矩阵P和卡尔曼增益K：
[0097]
[0098] 3.修正当前状态x及其误差协方差矩阵P的估计
[0099]
[0100]
[0101] 综上所示，本发明使用所提出的RAUKF算法在不同的环境下进行了模拟实验，如图2,3,4,5所示。实验结果表明，当USV中的传感器对先验测量噪声估计出现了偏差或在运行过程中传感器发生故障造成实际噪声逐渐增大时，本发明相比于传统UKF都可以提供更为精确的位置和航向角估计。因此本发明提出的鲁棒自适应UKF方法可以为USV组合导航定位提供一种有效的方法。
