CN103336525B - 随机系统高权值便捷ukf滤波方法 - Google Patents

Abstract

本发明涉及一种随机系统高权值便捷UKF滤波方法，其特征在于：步骤1：建立空间运动体数学模型并离散化，获得离散化后的状态方程和观测方程；步骤2：对状态值的初值进行设定；步骤3：构造n+1个加权点和相应的高权值,计算加权点状态预测值及状态预测值的均值和方差；步骤:4：根据加权点状态预测值、加权点状态预测值的均值和方差，利用UKF滤波方法对状态值和方差进行更新；步骤5：重复步骤3、步骤4进行迭代运算，获得一系列状态值。

Description

随机系统高权值便捷UKF滤波方法

技术领域

本发明涉及一种应用于空间运动体在随机干扰情况下的运动控制系统中的改进UKF（无迹卡尔曼）滤波方法。

背景技术

在运动控制系统中，空间运动体的运动往往受到随机干扰，如船舶在运动中受到随机海浪的干扰，导弹在空中运动受到气流影响等。加之测量仪器中也不可避免地存在着测量噪声，极小的测量误差就可能引起较大的控制误差，因此利用滤波技术进行优化估计通常是一种较有效的方法。通过对一系列带有观测噪声和干扰信号的观测数据进行处理，得到接近真实值的状态量，可以提高控制器的控制效果。

南京理工大学学报，2005年8月刊登的“基于平方根Unsecured卡尔曼滤波的无陀螺卫星的姿态估计”、中国优秀硕士学位论文全文数据库，公开的“UKF在水中目标跟踪系统中应用”等文献中涉及的技术方案，主要侧重改善滤波效果，提高估计精度，但提出的改进方法都能加了算法的运算量。在实际运动控制系统中，滤波技术在满足滤波要求的情况下，更希望采用较小的运算量来实现功能，进而提高控制效果。

发明内容

本发明目的在于提供一种随机系统高权值便捷UKF滤波方法，能够减少滤波计算中的运算量，提供精确有效的状态反馈信息。

实现本发明目的技术方案：

一种随机系统高权值便捷UKF滤波方法，其特征在于：

步骤1：建立空间运动体数学模型并离散化，获得离散化后的状态方程和观测方程；

步骤2：对状态值的初值进行设定；

步骤3：构造n+1个加权点和相应的权值W_i ^(m)，W_i ^(c)(i=0,1,2,…,n)，其中W_i ^(m)、W_i ^(c)

通过以下公式获得：

${W_i}^{\left(c\right)}=\left\{\begin{array}{c}\mathrm{\λ}/(n+\mathrm{\λ})+(1-a^2+\mathrm{\β}),i=0\\ 1/\left[(n+\mathrm{\λ})\right]+(1-a^2+\mathrm{\β}),i=1,L,n\end{array}\right.$

加权点通过如下公式获得：

$\mathrm{\χ}(k-1)={\left\{\begin{array}{c}\hat{X}(k-1)\\ \hat{X}(k-1)+{\left[\sqrt{(n+\mathrm{\λ})P_x(k-1)}\right]}_{2i-1}+{\left[\sqrt{(n+\mathrm{\λ})P_x(k-1)}\right]}_{2i}\\ \hat{X}(k-1)-{\left[\sqrt{(n+\mathrm{\λ})P_x(k-1)}\right]}_{2i-1}-{\left[\sqrt{(n+\mathrm{\λ})P_x(k-1)}\right]}_{2i}\end{array}\right\}}^T$

i=1,L,n

表示状态估计值P_x′(k-1)表示状态方差，缩放比例参数λ=a²(n+κ)-n，a用于设置这些点集到均值点的距离，κ是一个比例参数；

计算加权点状态预测值χi^-(k)、加权点状态预测值的均值和方差

步骤:4：根据加权点状态预测值χi^-(k)、加权点状态预测值的均值和方差

利用UKF滤波方法对状态值和方差进行更新；

步骤5：重复步骤3、步骤4进行迭代运算，获得一系列状态值。

步骤3中，通过以下公式计算加权点状态预测值的均值和方差

${\hat{x}}^{-}\left(k\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{\mathrm{\χ}}_i^{-}\left(k\right)$

$P_x^{-}\left(k\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}[{\mathrm{\χ}}_i^{-}\left(k\right)-{\hat{x}}^{-}\left(k\right)]{[{\mathrm{\χ}}_i^{-}\left(k\right)-{\hat{x}}^{-}\left(k\right)]}^T+Q\left(k\right)$

式中，Q(k)为系统干扰的方差矩阵。

步骤4中，获得新的测量值Y(k)后，利用UKF滤波方法对状态值和方差进行更新；

状态估计值更新： $\hat{X}\left(k\right)={\hat{x}}^{-}\left(k\right)+K[Y\left(k\right)-{\hat{Y}}^{-}\left(k\right)];$

状态方差更新： $P_X\left(k\right)=P_X^{-}\left(k\right)-{\mathrm{KP}}_Y\left(k\right)K^T;$

增益矩阵K=P_XY(k)P_Y ^-1(k);

加权测量值： ${\hat{Y}}^{-}\left(k\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}H\left[{\mathrm{\χ}}_i^{-}\left(k\right)\right];$

加权测量值方差：

$P_Y\left(k\right)=\underset{i-0}{\overset{n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}\left\{H\right[{\mathrm{\χ}}_i^{-}\left(k\right)]-{\hat{Y}}^{-}\left(k\right)\}{\left\{H\right[{\mathrm{\χ}}_i^{-}\left(k\right)]-{\hat{Y}}^{-}\left(k\right)\}}^T+R\left(k\right);$

式中，表示预测加权点的观测值，R(k)是观测噪声方差矩阵；

加权状态与加权测量值协方差：

$P_{\mathrm{XY}}\left(k\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}\{{\mathrm{\χ}}_i^{-}\left(k\right)-{\hat{x}}^{-}\left(k\right)\}{\left\{H\right[{\mathrm{\χ}}_i^{-}\left(k\right)-{\hat{Y}}^{-}\left(k\right)\left]\right\}}^T.$

本发明具有的有益效果：

与传统UT变换相比，本发明高权值UT变换使得加权点减少n个，将非中心采样点的权值定位原UT变换的2倍，同时每个非中心采样点包含两列方差信息，则高权值UT变换所得到的加权点点依然代表状态量的均值和方差，满足：

$\left[\begin{array}{c}\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{\mathrm{\ω}}_i-1\\ \underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{\mathrm{\ω}}_i{\mathrm{\χ}}_i-\stackrel{\‾}{X}\\ \underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{\mathrm{\ω}}_i({\mathrm{\χ}}_i-\stackrel{\‾}{X}){({\mathrm{\χ}}_i-\stackrel{\‾}{X})}^T-P_X\end{array}\right]=0$

本发明在保证提供精确有效的状态反馈信息前提下，能够大幅减少计算量。

附图说明

图1为本发明流程框图；

图2为船舶纵向姿态估计器在船舶系统作用示意图；

图3为基于船舶姿态估计算法示意图。

具体实施方式

步骤1：建立空间运动体数学模型并离散化，获得离散化后的状态方程和观测方程；

以船舶纵向姿态为例，纵向姿态估计器在船舶系统作用示意如图2所示。

船舶纵向运动主要有纵摇运动和升沉运动。设某船舶的纵向姿态运动用状态量取 $X={\left[\begin{array}{cccc}z& \stackrel{\·}{z}& \mathrm{\θ}& \stackrel{\·}{\mathrm{\θ}}\end{array}\right]}^T,$ 其中z代表船舶升沉位移，代表船舶升沉速度，θ代表船舶纵摇角，代表船舶纵摇角速度。

船舶纵向运动方程并进行离散化，离散化后状态方程和观测方程表示为：

$\left\{\begin{array}{c}X(k+1)=\mathrm{\Φ}\left(X\left(k\right)\right)+\mathrm{\Γ}\·W\left(k\right)\\ Y\left(k\right)=H\left(X\left(k\right)\right)+V\left(k\right)\end{array}\right.$

状态方程中，船舶纵向运动控制函数用Φ(X(k))=F(X(k))+U(X(k))表示，海浪干扰考虑海浪对船舶作用的干扰力和干扰力矩用W(X(k))表示，假设随机海浪干扰为白噪声W(k)～N(0,Q(k))。

观测方程中 $H\left(X\left(k\right)\right)={\left[\begin{array}{cc}\stackrel{\·\·}{z}& \stackrel{\·}{\mathrm{\θ}}\end{array}\right]}^T,$ 为船舶升沉加速度，为纵摇角速度，V(k)为系统观测噪声，即传感器的测量噪声，不失一般性，假设传感器噪声为白噪声V(k)～N(0,R(k))。

步骤2：对状态值的初值进行设定；

将状态X(0)初值的统计特性为：

$\left\{\begin{array}{c}X\left(0\right)=E\left[X\left(0\right)\right]\\ P\left(0\right)=E\left\{\right[\left(0\right)-X\left(0\right)\left]{[X\left(0\right)-X\left(0\right)]}^T\right\}\end{array}\right.---\left(2\right)$

在不知道船舶具体运动规律时可以选取：P(0)为ｎ维单位阵，其中ｎ为系统维数。

步骤3：构造n+1个加权点和相应的权值W_i ^(m)，W_i ^(c)(i=0,1,2,…,n)，其中W_i ^(m)、W_i ^(c)

通过以下公式获得：

$W_i^{\left(m\right)}=\left\{\begin{array}{c}\mathrm{\λ}/(n+\mathrm{\λ}),i=0\\ 1/\left[(n+\mathrm{\λ})\right],i=1,L,n\end{array}\right.$

$W_i^{\left(c\right)}=\left\{\begin{array}{c}\mathrm{\λ}/(n+\mathrm{\λ})+(1-a^2+\mathrm{\β}),i=0\\ 1/\left[(n+\mathrm{\λ})\right]+(1-a^2+\mathrm{\β}),i=1,\·\·\·,n\end{array}\right.$

加权点通过如下公式获得：

$\mathrm{\χ}(k-1)={\left\{\begin{array}{c}\hat{X}(k-1)\\ \hat{X}(k-1)+{\left[\sqrt{(n+\mathrm{\λ})P_x(k-1)}\right]}_{2i-1}+{\left[\sqrt{(n+\mathrm{\λ})P_x(k-1)}\right]}_{2i}\\ \hat{X}(k-1)-{\left[\sqrt{(n+\mathrm{\λ})P_x(k-1)}\right]}_{2i-1}-{\left[\sqrt{(n+\mathrm{\λ})P_x(k-1)}\right]}_{2i}\end{array}\right\}}^T$

i=1,L,n

表示状态估计值P_x′(k-1)表示状态方差，缩放比例参数λ=a²(n+κ)-n，a用于设置这些点集到均值点的距离，κ是一个比例参数；

计算加权点状态预测值χi^-(k)、加权点状态预测值的均值和方差

${\hat{x}}^{-}\left(k\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{\mathrm{\χ}}_i^{-}\left(k\right)$

$P_x^{-}\left(k\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}[{\mathrm{\χ}}_i^{-}\left(k\right)-{\hat{x}}^{-}\left(k\right)]{[{\mathrm{\χ}}_i^{-}\left(k\right)-{\hat{x}}^{-}\left(k\right)]}^T+Q\left(k\right)$

式中，Q(k)为系统干扰，即海浪干扰的方差矩阵。

步骤:4：获得新的测量值Y(k)后，利用UKF滤波方法对状态值和方差进行更新；

状态估计值更新： $\hat{X}\left(k\right)={\hat{x}}^{-}\left(k\right)+K[Y\left(k\right)-{\hat{Y}}^{-}\left(k\right)];$

状态方差更新： $P_X\left(k\right)=P_X^{-}\left(k\right)-{\mathrm{KP}}_Y\left(k\right)K^T;$

增益矩阵K=P_XY(k)P_Y ^-1(k);

加权测量值： ${\hat{Y}}^{-}\left(k\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}H\left[{\mathrm{\χ}}_i^{-}\left(k\right)\right];$

加权测量值方差：

$P_Y\left(k\right)=\underset{i-0}{\overset{n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}\left\{H\right[{\mathrm{\χ}}_i^{-}\left(k\right)]-{\hat{Y}}^{-}\left(k\right)\}{\left\{H\right[{\mathrm{\χ}}_i^{-}\left(k\right)]-{\hat{Y}}^{-}\left(k\right)\}}^T+R\left(k\right);$

式中，表示预测加权点的观测值，R(k)是观测噪声方差矩阵；

加权状态与加权测量值协方差：

$P_{\mathrm{XY}}\left(k\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}\{{\mathrm{\χ}}_i^{-}\left(k\right)-{\hat{x}}^{-}\left(k\right)\}{\left\{H\right[{\mathrm{\χ}}_i^{-}\left(k\right)-{\hat{Y}}^{-}\left(k\right)\left]\right\}}^T$

步骤5：重复步骤3、步骤4进行迭代运算，获得一系列状态值。

Claims (1)

1.一种随机系统高权值便捷UKF滤波方法，其特征在于：

步骤1：建立数学模型并离散化，获得离散化后的状态方程和观测方程；

船舶纵向运动包括纵摇运动和升沉运动，设船舶纵向运动用状态量 表示，其中z代表船舶升沉位移、代表船舶升沉速度、θ代表船舶纵摇角、代表船舶纵摇角速度，

将船舶纵向运动方程进行离散化，离散化后状态方程和观测方程表示为：

状态方程中，船舶纵向运动控制函数用ΦX(k)表示，海浪干扰考虑海浪对船舶作用的干扰力和干扰力矩，假设随机海浪干扰为白噪声W(k)～N(0,Q(k))，

观测方程中 为船舶升沉加速度、为纵摇角速度，V(k)为系统观测噪声，即传感器的测量噪声，不失一般性，假设传感器的测量噪声为白噪声V(k)～N(0,R(k))；

步骤2：对状态量的初值进行设定；

步骤3：构造n+1个加权点和相应的权值其中

通过以下公式获得：

加权点通过如下公式获得：

表示状态估计值，P_X(k-1)表示状态方差，缩放比例参数λ＝a²(n+κ)-n，a用于设置这些点集到均值点的距离，κ是一个比例参数；

计算加权点状态预测值加权点状态预测值的均值和方差

步骤:4：根据加权点状态预测值加权点状态预测值的均值和方差利用UKF滤波方法对状态估计值和状态方差进行更新；

步骤5：重复步骤3、步骤4进行迭代运算，获得一系列状态值；

步骤3中，通过以下公式计算加权点状态预测值的均值和方差

式中，Q(k)为系统随机海浪干扰的方差矩阵；

步骤4中，获得新的测量值Y(k)后，利用UKF滤波方法对状态估计值和状态方差进行更新；

状态估计值更新：

状态方差更新：

增益矩阵K＝P_XY(k)P_Y ^-1(k)；

加权测量值：

加权测量值方差：

式中，表示预测加权点的观测值，R(k)是观测噪声方差矩阵；

加权状态与加权测量值协方差：