基于Huber估计的鲁棒自适应滤波和状态估计方法

摘要

本发明提供的是一种基于Huber估计的鲁棒自适应滤波和状态估计方法。首先通过在线估计一步预测双重不确定性系统的协方差矩阵的上界来计算估计值，只对时间更新过程进行修改，同时采用线性矩阵不等式的方法进行自适应参数调整，得到应对不确定性模型的自适应无迹信息滤波器；然后应用矩阵求逆定理并采用Huber估计的方法对量测更新过程进行修改，得到最终的估计值。本发明的滤波和状态估计的精度高，稳定性好。所提出的鲁棒自适应滤波和状态估计方法解决了双重不确定无人艇的滤波和状态估计问题，即保留了信息滤波器结构的优点有提高了滤波和状态估计的精度和稳定性，计算简单应用方便。

说明书

技术领域

本发明涉及的是一种鲁棒自适应滤波和状态估计方法，具体地说是一种基于Huber估计的鲁棒自适应滤波和状态估计方法。

背景技术

无迹信息滤波器(UIF)是一种新兴的滤波和状态估计方法，本质上是一种通过协方差矩阵的逆来更新的Kalman滤波器，鉴于无迹信息滤波对协方差矩阵初值无要求、有比普通Kalman滤波器更易于计算的更新方程及易于多源融合的扩展等优点得到广泛关注。然而对于无人艇不确定运动模型、海洋干扰模型和不确定传感器量测噪声同时存在的双重不确定系统时，常规无迹信息滤波器的滤波精度会明显下降甚至无法收敛。而且现有的海浪滤波和状态估计技术大多针对某一不确定性问题进行研究并提出了较好的解决方法，对于上述包含两种及以上不确定因素的双重不确定系统还未给出有效的滤波和状态估计方法。

双重不确定系统的滤波和状态估计问题广泛存在于运载器(水面无人艇、水下航行器等)控制领域，备受许多学者关注。

发明内容

本发明的目的在于提供一种能够保证双重不确定系统滤波和状态估计的精度和稳定性的基于Huber估计的鲁棒自适应滤波和状态估计方法。

本发明的目的是这样实现的：

首先通过在线估计一步预测双重不确定性系统的协方差矩阵的上界来计算估计值，只对时间更新过程进行修改，同时采用线性矩阵不等式的方法进行自适应参数调整，得到应对不确定性模型的自适应无迹信息滤波器；然后应用矩阵求逆定理并采用Huber估计的方法对量测更新过程进行修改，得到最终的估计值，具体包括如下步骤：

(1)、初始化，初始状态估计值设为0，初始协方差矩阵选择某一常值矩阵，不能选择零矩阵，满足

(2)、时间更新，按照常规无迹信息滤波的时间更新过程计算和表示估计状态变量的均值，P_k|k-1表示一步预测协方差；

(3)、自适应时间更新，计算协方差上界估计值后，用代替P_k|k-1，结合无迹信息滤波的量测更新方程，求得最终的估计值及其协方差矩阵采用线性矩阵不等式方法进行在线自适应参数调整；

(4)、基于Huber估计的量测更新，应用矩阵求逆定理并在量测更新部分引入Huber估计方法，更新过程中的P_k|k-1均替换为第(3)步的且中间变量也相应地增加上标(u)，最终得到估计结果和

本发明还可以包括：

1、所述的双重不确定系统的不确定性集成在一起表示为：

其中，为无人艇状态变量，u为推进器推力和环境作用力，为确定噪声，E为适维常数矩阵，H为适维常数矩阵，非线性函数f(·)包含未知建模误差、时变参数不确定性因素，v为适维量测噪声、指电罗经和GPS的传感器测量噪声；

双重不确定系统的非线性离散模型为：

x_k＝f_a(x_k-1,u_k-1)+Δ+E_k-1w_k-1

y_k＝H_kx_k+v_k，

其中，x_k∈Rⁿ和y_k∈R^m分别为状态变量和量测变量，函数f_a(·)表示标称模型，为模型中的确定部分；Δ表示建模误差，且有界，设||Δ||≤χ,χ≥0；w_k和v_k为相互独立高斯白噪声，方差分别为Q_k和R_k，维数分别为n_v和n_n；k为采样数、为非负整数。

2、所述按照常规无迹信息滤波的时间更新过程计算和P_k|k-1具体包括：

设置初值使得的初值满足P₀^u≥P₀，那么和都满足以上不等式，根据来得到当前时刻的和

经过一系列不等式缩放，得一步预测协方差的保守上界，按照其结构进行参数化得

其中，P_k|k-1为一步预测值协方差，λ_k,α,β为待设计的滤波器参数，tr(·)表示矩阵的迹，I表示适维单位阵；

首先通过调节参数α,β对建模误差进行补偿，其次采用线性矩阵不等式方法调整自适应无迹信息滤波参数λ_k；

由一步预测值的协方差得新息协方差矩阵的预测值为：

同时，通过引入一个加权因子μ∈[0,1]，得新息协方差矩阵观测值的无偏估计为

其中，为新息方程，且μ的大小会影响滤波器对噪声特性和模型不确定性的敏感性，当μ＝0时，仅由当前时刻的观测值决定，当μ＝1时，还会受到历史观测值影响；

通过如下最优问题来调节λ_k，同时参数更新时间间隔通过下式的计算间隔确定：

其中，参数λ_k在线调节，参数α,β离线调节；

当计算出后，用其代替P_k|k-1，然后采用Huber估计方法的量测更新方程求得最终的估计值及其协方差矩阵

3、Huber估计的鲁棒无迹信息滤波器是采用如下方法设计的：

初始化及时间更新部分与常规无迹信息滤波和状态估计算法一致，首先k时刻的估计值表示为

其中Δ_k表示一步预测误差，协方差为P_k|k-1；

根据常规无迹信息滤波和状态估计算法的推导过程及结果，存在如下映射

其中选取线性的量测方程，

线性回归模型如下：

定义

则

其中，为残差，

令

其中，表示常规无迹信息滤波量测更新后的初始估计值；

Huber估计是通过最小化目标函数J来估计被估量，则有如下目标函数

其中，r_i表示残差，σ_i是比例因子为单位阵，残差是被估量的函数，

为使目标函数最小，则

其中，Huber估计的ρ函数、核函数ψ(r)和权值函数q(r)分别表示如下，γ为调节参数，

则最小目标函数为：

其中，

通过迭代法求最小目标函数，迭代公式为：

其中，j为迭代次数、取j＝1，初值取或者取常规无迹信息滤波量测更新后的估计值，

然后重新定义信息滤波中的信息矩阵和信息向量：

并修改协方差矩阵P_k|k-1的计算公式为：

最后按照信息滤波的形式进行量测更新：

其中的信息状态增量及其关联矩阵，为：

本发明针对双重不确定性无人艇的滤波和状态估计问题，提出了一种基于Huber估计的鲁棒自适应滤波和状态估计方法，保证双重不确定系统滤波和状态估计的精度和稳定性。主要包括：

一、双重不确定系统

无人艇模型参数会随着船速、水深和负载等因素的变化而变化，因此无人艇数学模型存在不确定性；同时不管无人艇处于何种运动状态，均要受到高低频海浪干扰，其建模过程存在未知建模误差、时变参数等不确定性因素；对于不确定量测噪声，当传感器测量数据没有受到干扰时，可以采用高斯白噪声近似模拟，但实际测量过程中，测量数据由于受到各种干扰的污染，使得量测噪声为非高斯分布。因此系统进行滤波和状态估计时实际存在两种及以上不确定因素，即双重不确定系统。

二、自适应无迹信息滤波器

对于不确定性模型的建模误差，引入估计值状态协方差的上界，只对时间更新过程进行修改，同时采用线性矩阵不等式的方法进行自适应参数调整，进而推导出应对模型不确定性的自适应无迹信息滤波和状态估计方法。

三、利用Huber估计的鲁棒无迹信息滤波器

对于不确定量测噪声，应用矩阵求逆定理并采用Huber估计的方法对量测更新过程进行修改，即保留了信息滤波器量测更新的结构又解决了量测噪声非高斯分布时的滤波和状态估计问题。

本发明包括以下有益效果：

1、本发明的无迹信息滤波器对协方差矩阵初值无要求、易于多源融合的扩展。

2、本发明对建模误差有鲁棒性，通过在线估计一步预测协方差矩阵的上界来计算估计值，并且只修改时间更新过程。

3、本发明应用矩阵求逆定理并引入Huber估计方法，只对量测更新过程进行修改，不会在Huber估计中引入线性化误差，不会改变原有无迹信息滤波器的结构。

4、本发明所述的基于Huber估计的无迹信息滤波和状态估计方法，可以保证双重不确定无人艇的滤波精度和稳定性。

本发明涉及的是一种基于Huber估计的鲁棒自适应无迹信息滤波和状态估计(Huber-based Adaptive Unscented Information Filter，即HAUIF)技术，应用于欠驱动水面无人艇，即具有双重不确定性无人艇的滤波和状态估计。基于Huber估计和无迹信息滤波是一种处理具有双重不确定系统的滤波和状态估计的有效方法。这种方法通过在线估计一步预测协方差矩阵的上界来计算估计值，只对时间更新过程进行修改，同时采用线性矩阵不等式的方法进行自适应参数调整，推导出应对不确定性模型的自适应无迹信息滤波器(AUIF)；然后应用矩阵求逆定理并采用Huber估计的方法对量测更新过程进行修改，得到最终的估计值，即保留了信息滤波器量测更新的结构又提高了量测噪声非高斯分布时的滤波精度。

附图说明

图1为本发明所述滤波和状态估计方法的结构框图；

图2为无人艇的局部坐标系与船体坐标系示意图；

图3为双重不确定系统的常规无迹信息滤波器(UIF)估计误差；

图4为双重不确定系统的自适应无迹信息滤波器(AUIF)估计误差；

图5为双重不确定系统的HAUIF估计误差。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合图1至图5举例对本发明作进一步详细的说明。

结合图1，本发明所述滤波和状态估计方法主要包括如下步骤：

1、初始化，初始状态估计值设为0，初始协方差矩阵根据情况选择某一常值矩阵，不能选择零矩阵，因为需要满足

2、时间更新，按照常规无迹信息滤波的时间更新过程计算和P_k|k-1；

3、自适应时间更新，计算协方差上界估计值后，用其代替P_k|k-1，结合无迹信息滤波的量测更新方程即可求得最终的估计值及其协方差矩阵采用线性矩阵不等式方法进行在线自适应参数调整；

4、基于Huber估计的量测更新，应用矩阵求逆定理并在量测更新部分引入Huber估计方法，注意更新过程中的P_k|k-1均需要替换为第3步的且中间变量也需相应地增加上标(u)，最终得到估计结果和

本实施方式所述的基于Huber估计的鲁棒自适应滤波和状态估计方法的特点包括：

一、双重不确定系统

无人艇模型参数会随着船速、水深和负载等因素的变化而变化，因此无人艇数学模型存在不确定性；同时不管无人艇处于何种运动状态，均要受到高低频海浪干扰，其建模过程存在未知建模误差、时变参数等不确定性因素；对于不确定量测噪声，当传感器测量数据没有受到干扰时，可以采用高斯白噪声近似模拟，但实际测量过程中，测量数据由于受到各种干扰的污染，使得量测噪声为非高斯分布。因此系统进行滤波和状态估计时实际存在两种及以上不确定因素，即双重不确定系统。

二、自适应无迹信息滤波器

对于不确定性模型的建模误差，引入估计值状态协方差的上界，只对时间更新过程进行修改，同时采用线性矩阵不等式的方法进行自适应参数调整，进而推导出应对模型不确定性的自适应无迹信息滤波和状态估计方法。

三、利用Huber估计的鲁棒无迹信息滤波器

对于不确定量测噪声，应用矩阵求逆定理并采用Huber估计的方法对量测更新过程进行修改，即保留了信息滤波器量测更新的结构又解决了量测噪声非高斯分布时的滤波和状态估计问题。

结合图2所述的双重不确定系统如下：

1、不确定无人艇数学模型：

通过航向自动舵及GPS设置的航迹点来间接控制无人艇的航迹，无人艇航向模型可写为：

其中，ψ为艏向角，r为艏摇角速度，为其一阶导数，δ为实际舵角，H_N(r)为描述操纵特性的非线性函数，b表示舵角偏移量，其作用是为了抵消慢变干扰，w₁∈R表示系统噪声，T,K为待设计参数。

为方便描述，可用一个统一的形式表示为：

其中，函数f₁(·)为非线性函数，x₀＝[ψ,r]^T,u＝δ,w_p＝w₁。

2、不确定海洋干扰模型：

高频海浪干扰为一阶波浪力，可使用二阶滤波器对其进行建模，状态空间形式为：

η_w＝C_wξ

其中，ξ∈R为高频运动状态，η_w∈R为高频海浪引起的无人艇艏向角变化，w_w为零均值白噪声过程，A_w,E_w,C_w为适维矩阵。

低频海浪干扰为二阶波浪力、海风和海流等，可采用在输入中叠加一个慢变信号的方式来模拟，慢变干扰b采用一阶马尔科夫过程来模拟：

其中，T_b为适维时间常数，一般取值较大，w_b为适维高斯白噪声。

不确定性集成在一起可表示为：

y＝Hx+v

其中，为无人艇状态变量，u为推进器推力和环境作用力，为确定噪声，E为适维常数矩阵，H为适维常数矩阵，非线性函数f(·)包含未知建模误差、时变参数等不确定性因素，v为适维量测噪声，主要指电罗经和GPS的传感器测量噪声。

综上双重不确定无人艇非线性离散模型为：

x_k＝f_a(x_k-1,u_k-1)+Δ+E_k-1w_k-1

y_k＝H_kx_k+v_k

其中，x_k∈Rⁿ和y_k∈R^m分别为状态变量和量测变量，函数f_a(·)表示标称模型，为模型中的确定部分；Δ表示建模误差，且有界，假设||Δ||≤χ,χ≥0；w_k和v_k为相互独立高斯白噪声，方差分别为Q_k和R_k，维数分别为n_v和n_n；k为采样数(非负整数)，L＝n+n_v。

结合图1图3，所述的常规无迹信息滤波和状态估计算法为：

1、初始化：

式中，表示估计状态变量的初值，表示全维状态变量的初值，表示全维协方差矩阵的初值。

2、计算系统状态sigma点：

其中，λ＝α²n-n为比例系数，α∈[0,1]通常取一个很小的数。

3、时间更新：

其中，表示估计状态变量的均值，P_k|k-1表示一步预测协方差，χ_i为状态变量对应的sigma点，w_i为sigma点的权值计算如下

4、定义中间变量(信息矩阵和信息向量)：

5、量测更新：

式中，P_xy表示协方差矩阵。

结合图1图4，步骤二中自适应无迹信息滤波器的具体设计过程为：

对不确定模型的建模误差，引入在线估计一步预测协方差矩阵的上界，同时采用线性矩阵不等式方法进行自适应参数调整，使得估计结果对模型不确定性具有鲁棒自适应性。记为估计值协方差上界，为一步预测值的协方差上界，则有：

设置足够大的初值使得的初值满足那么和都将满足以上不等式，根据来得到当前时刻的和

经过一系列的不等式缩放，可得一步预测协方差的较为保守的上界，按照其结构进行参数化可得

其中，P_k|k-1为一步预测值协方差，λ_k,α,β为待设计的滤波器参数，tr(·)表示矩阵的迹，I表示适维单位阵。

分析上式可知：第一项λ_kP_k|k-1表示计算过程尽可能大地利用一步预测协方差矩阵P_k|k-1，而P_k|k-1可以通过时间更新过程得到，这一项可以应对建模误差对时间更新过程的影响；第二项与状态估计结果的协方差上界有关，可以应对建模误差对最终估计结果(量测更新之后)的影响；第三项是一个常值，可以应对模型中的常值不确定性。对于非线性离散系统模型的建模误差项假设满足||Δ||≤χ,χ≥0，是无需知道其上界具体值的，并且其在实际中也较难获得。首先通过调节参数α,β即可对建模误差进行较好的补偿，其次采用线性矩阵不等式方法调整自适应无迹信息滤波参数λ_k：

由一步预测值的协方差上界知信息协方差矩阵的预测值要大于其观测值。由一步预测值的协方差可得，新息协方差矩阵的预测值为：

同时，通过引入一个加权因子μ∈[0,1]，可得新息协方差矩阵观测值的无偏估计为

其中，为新息方程，且μ的大小会影响滤波器对噪声特性和模型不确定性的敏感性，当μ＝0时，仅由当前时刻的观测值决定，当μ＝1时，还会受到历史观测值影响。

为使得减小估计的保守性，可通过如下最优问题来调节λ_k，同时参数更新时间间隔可以通过下式的计算间隔确定：

其中，参数λ_k在线调节，参数α,β离线调节。

由于为一步预测值的协方差上界不等式成立的必要条件而不是充分条件，所以α,β对滤波器的性能非常重要。当计算出后，用其代替P_k|k-1，然后采用Huber估计方法的量测更新方程即可求得最终的估计值及其协方差矩阵

结合图1图5，利用Huber估计的鲁棒无迹信息滤波器的具体设计过程为：

对于不确定量测噪声，应用矩阵求逆定理并在量测更新部分引入Huber估计方法，保留了无迹信息滤波量测更新的结构。

初始化及时间更新部分与常规无迹信息滤波和状态估计算法一致，首先k时刻的估计值可表示为

其中Δ_k表示一步预测误差，协方差为P_k|k-1。

根据常规无迹信息滤波和状态估计算法的推导过程及结果，存在如下映射

其中选取线性的量测方程。

线性回归模型如下：

定义

则有

其中，为残差。

令

其中，表示常规无迹信息滤波量测更新后的初始估计值。

Huber估计的思想是通过最小化目标函数J来估计被估量。则有如下目标函数

其中，r_i表示残差，σ_i是比例因子为单位阵，残差一般是被估量的函数。

为使目标函数最小，则需

其中，Huber估计的ρ函数、核函数ψ(r)和权值函数q(r)分别表示如下，γ为调节参数，表达式如下

则最小目标函数为：

其中，

通过迭代法求最小目标函数，迭代公式为：

其中，j为迭代次数，一般取j＝1即可，初值取或者取常规无迹信息滤波量测更新后的估计值。

然后重新定义信息滤波中的信息矩阵和信息向量：

并修改协方差矩阵P_k|k-1的计算公式为：

最后则可按照信息滤波的形式进行量测更新：

但需修改其中的信息状态增量及其关联矩阵，为：

本发明适用于包括不确定无人艇模型、海洋干扰模型和不确定传感器量测噪声同时存在的双重不确定无人艇的滤波和状态估计。提出一种基于Huber估计的鲁棒自适应滤波和状态估计方法：这种方法通过在线估计一步预测协方差矩阵的上界来计算估计值，只对时间更新过程进行修改，同时采用线性矩阵不等式的方法进行自适应参数调整，推导出应对不确定模型的自适应无迹信息滤波器(AUIF)；然后应用矩阵求逆定理并采用Huber估计的方法对量测更新过程进行修改，解决了量测噪声非高斯分布时的滤波和状态估计问题。通过附图3-5比较UIF、AUIF和HAUIF的估计误差效果图，可知HAUIF的滤波和状态估计的精度高，稳定性好。最终本发明所提出的鲁棒自适应滤波和状态估计方法解决了双重不确定无人艇的滤波和状态估计问题，即保留了信息滤波器结构的优点有提高了滤波和状态估计的精度和稳定性，计算简单应用方便。