CN105513091A - Dual-Bayesian estimation-based motion state estimation method for underwater motion body - Google Patents

Dual-Bayesian estimation-based motion state estimation method for underwater motion body Download PDF

Info

Publication number
CN105513091A
CN105513091A CN 201510835168 CN201510835168A CN105513091A CN 105513091 A CN105513091 A CN 105513091A CN 201510835168 CN201510835168 CN 201510835168 CN 201510835168 A CN201510835168 A CN 201510835168A CN 105513091 A CN105513091 A CN 105513091A
Authority
CN
Grant status
Application
Patent type
Prior art keywords
motion
estimation
bayesian
underwater
body
Prior art date
Application number
CN 201510835168
Other languages
Chinese (zh)
Inventor
刘厂
赵俊翔
高峰
赵玉新
赵美珍
魏宇
Original Assignee
哈尔滨工程大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date

Links

Abstract

The invention belongs to the motion state estimation field and especially provides a dual-Bayesian estimation-based motion state estimation method for an underwater motion body. According to the method, a motion state estimation model for an underwater motion body is established: firstly, the motion speed and the motion direction of the underwater motion body at the previous moment are calculated based on observed quantities; secondly, the parameters of Bayesian estimation are updated based on the observation information at the next moment; thirdly, based on the motion estimation method of the adaptive Bayesian estimation, the motion speed and the motion direction of the underwater motion body at the current moment are estimated; finally, based on the location of the underwater motion body at the current moment and the estimation result of the adaptive Bayesian estimation, the location of the underwater motion body at the next moment is obtained through the trace-less Bayesian motion estimation method. According to the technical scheme of the invention, based on the complex motion characteristics of the underwater motion body, a motion model that is fitted to the underwater motion body is obtained according to the dual-Bayesian estimation method. Therefore, the motion state of the underwater motion body can be accurately known. The estimation accuracy of the above algorithm is higher than that of the single-stage motion state estimation method.

Description

一种基于双贝叶斯估计的水下运动体运动状态估计方法 Motion state based on double submerged bodies Bayesian estimation method of estimation

技术领域 FIELD

[0001] 本发明属于运动状态估计领域,特别是一种基于双贝叶斯估计的水下运动体状态估计方法。 [0001] The present invention belongs to the field of motion estimation, in particular, a method of estimating the state of submerged bodies bis Bayesian estimation.

背景技术 Background technique

[0002] 要实现对水下运动体的跟踪,需要对其运动状态进行实时估计。 [0002] To achieve the tracking of submerged bodies, the need for real-time estimation of its state of motion. 在对于水下运动体的运动状态估计问题中,由于水下运动体运动复杂,机动性强,简单的针对固定模型的估计方法不能保持估计精度,并容易发散。 For the estimation of the state of motion of submerged bodies, since the moving body underwater complex, highly mobile, simple method for estimating fixed model estimation accuracy can not be maintained, and easily dissipate. 在模型方面,由于水下运动体运动速度较快,而多项式模型计算过于复杂,在实时性问题中无法应用。 In the model, since the underwater moving body faster, and polynomial model calculation is too complex to be used in real-time problems. 单纯的匀速(CV)或者联合转弯(CT)模型则太过简单,无法准确描述水下运动体的运动规律。 Simple uniform (CV) joint or turn (CT) model is too simple, can not accurately describe the movement of underwater moving body. 在常用的运动状态估计算法中,粒子滤波方法存在收敛缓慢的缺陷,因此也无法在水下运动体运动状态估计方法中使用,单纯的无迹卡尔曼或者容积卡尔曼算法能够很好的解决跟踪非线性目标的问题,但无法适应快速机动的水下运动体状态变化。 Algorithm, there is slow convergence defect particle filter, and therefore can not be used in motion estimation method of submerged bodies can be a good solution commonly used to track the motion state estimation, pure unscented Kalman Kalman algorithm or volume nonlinear target, but can not adapt to the rapid mobility of submerged bodies state changes.

[0003] 目前在运动状态估计和目标跟踪等领域的研究中,以单级估计占绝大多数。 [0003] In the current state of motion estimation and target tracking and other areas of study, a single-stage estimate the majority. 例如, 公开号为CN104574439A的专利公开了一种融合卡尔曼滤波与TLD算法的目标跟踪方法,利用改进的卡尔曼滤波器增强TLD算法,提高系统的可靠性。 For example, Publication No. discloses a method of object tracking and TLD fusion Kalman filter algorithm CN104574439A patent, the use of the improved Kalman filter TLD enhancement algorithm to improve system reliability. 公开号为CN104408744A的专利公开了一种用于目标跟踪的强跟踪容积卡尔曼滤波方法,利用强跟踪滤波器应对系统突变。 Patent Publication No. CN104408744A discloses a method for target tracking strong tracking Kalman filter volume, using a strong tracking filter response system mutation. 本专利针对水下运动体的运动模型,提出一种基于双贝叶斯估计的水下运动体状态估计方法。 This patent submerged bodies for the motion model is proposed submerged bodies state estimation method based on double Bayesian estimation.

发明内容 SUMMARY

[0004] 本发明的目的是为了解决水下运动体运动状态估计精度较低的问题,使用两级贝叶斯估计,提供了一种基于双贝叶斯估计的水下运动体运动状态估计方。 [0004] The object of the present invention is to solve the problems of submerged bodies motion estimation less accurate, the use of two Bayesian estimation, a Bayesian estimation based on two motion estimation direction submerged bodies .

[0005] 本发明的目的是这样实现的: [0005] The object of the present invention is implemented as follows:

[0006] (1)建立水下运动体的运动状态估计模型: [0006] (1) the establishment of an underwater moving body motion estimation model:

[0007] (1.1)建立水下运动体的观测模型: ξ - R cos E cos A + x [0007] The observation model (1.1) to establish underwater moving body: ξ - R cos E cos A + x

[0008] - η -- /?sin E -\-v ζ:~ /?cos£sin A. + z [0008] - η - / sin E - \ - v ζ:?? ~ / Cos £ sin A. + z

[0009] (1.2)建立水下运动体的运动模型ξ - vcos^cos^ [0009] (1.2) ξ motion model of submerged bodies - vcos ^ cos ^

[0010] < ή -- vsin φοο^θ ζ [0010] <ή - vsin φοο ^ θ ζ

[0011] (1.3)建立对水下运动体速度大小和方向进行估计的自适应贝叶斯估计器系统模型: [0011] (1.3) establish adaptive system model Bayesian estimation underwater speed and direction of motion estimation body:

[0012] [0012]

[0013] [0013]

Figure CN105513091AD00051

[0014] (1.4)建立对水下运动体位置进行估计的无迹贝叶斯估计器系统模型: [0014] (1.4) for establishing the position of submerged bodies estimate unscented Bayesian estimator system Model:

[0015] Xk+i = f(Xk, Φ ,9,v)+ffk [0015] Xk + i = f (Xk, Φ, 9, v) + ffk

[0016] yk = HXk+Vk [0016] yk = HXk + Vk

[0017] (2)利用观测量解算出前一时刻水下运动体的速度大小及方向: [0017] (2) using the observations of submerged bodies solves for the velocity magnitude and direction of a previous time:

[0018] 通过观测量解算出前一时刻水下运动体的速度及其方向; [0018] Observations by calculating the speed and direction of the solution of submerged bodies of a previous time;

[0019] (3)利用下一时刻的观测信息更新贝叶斯估计参数: [0019] (3) next time observation using the Bayesian information update parameters:

[0020] 在获得下一时刻的观测信息后,对贝叶斯估计的参数进行更新;分别进行自适应估计器和无迹估计器的相关参数更新; [0020] After obtaining information on the next observation time, Bayesian estimation parameter update; adaptive parameters were estimated and the estimator updates unscented;

[0021 ] (3.1)更新自适应估计器的参数,将水下运动体的角加速度和加速度的变化量视作系统噪声,使用噪声估计器进行估计; [0021] (3.1) updating the adaptive parameter estimator, the submerged bodies and the angular acceleration change amount of acceleration is regarded as the system noise using the noise estimator estimates;

[0022] (3.2)利用观测信息更新无迹估计器的参数; [0022] (3.2) observed by using the updated parameter information without a trace estimator;

[0023] (4)利用自适应贝叶斯运动估计方法,估计出水下运动体当前时刻的速度大小及方向; [0023] (4) using a Bayesian adaptive motion estimation method to estimate the velocity magnitude and direction of the current time of submerged bodies;

[0024] 通过自适应贝叶斯运动估计方法中的估计方程,得到下一时刻水下运动体的速度大小和方向的估计值; [0024] Estimation by Adaptive Bayesian estimation equation of motion to obtain the estimated value of the speed and direction of the next time of submerged bodies;

[0025] (5)利用水下运动体当前时刻位置和自适应贝叶斯的估计结果,通过无迹贝叶斯运动估计方法,得到下一时刻的水下运动体位置。 [0025] (5) using the current time location estimation result and an adaptive Bayesian submerged bodies, Bayesian estimation method of motion without a trace, to obtain a next time underwater moving body position.

[0026]本发明的有益效果在于: [0026] Advantageous effects of the present invention:

[0027] 针对水下运动体运动情况复杂的特点,利用双贝叶斯估计的方法,首先自适应地估计水下运动体的速度大小和方向,然后对水下运动体的位置进行估计,该方法贴合水下运动体的运动模型,能够精确把握水下运动体运动状态。 [0027] The submerged bodies for movement complex features, using the method of double Bayesian estimation, firstly adaptively estimated speed and direction of submerged bodies, then the position estimate of submerged bodies, which the method of bonding a motion model of submerged bodies, can accurately grasp the state of motion of submerged bodies. 算法的估计精度高于单级运动状态估计方法。 Estimation accuracy of the algorithm is higher than a single-stage motion estimation method.

附图说明 BRIEF DESCRIPTION

[0028] 图1为双贝叶斯运动状态估计流程图。 [0028] FIG. 1 is a flowchart of a double Bayesian estimation motion.

具体实施方式 detailed description

[0029] 下面结合附图对本发明做进一步描述。 [0029] The following figures further described in conjunction with the present invention.

[0030] 本发明涉及运动状态估计领域,特别是提出一种基于双贝叶斯估计的水下运动体运动状态估计方法。 [0030] The present invention relates to the field of motion estimation, in particular, to provide a dual motion estimation Bayesian estimation based on the submerged bodies. 具体包括建立估计模型,利用观测量解算出前一时刻水下运动体的速度及其方向,利用下一时刻的观测信息更新贝叶斯估计参数,利用自适应贝叶斯,估计出水下运动体当前时刻的速度大小及方向,利用水下运动体当前时刻位置和自适应贝叶斯的估计结果,通过无迹贝叶斯运动估计方法,得到下一时刻的水下运动体位置五个具体步骤。 Comprises establishing estimation model using observations solves for the direction and speed of movement of the underwater body of the previous time, the next time using the observation information update Bayesian estimation parameters, the adaptive Bayesian estimate the submerged bodies velocity magnitude and direction of the current time, the current time position estimation result using submerged bodies and adaptive Bayesian, Bayesian estimation method of motion without a trace, to obtain five specific steps underwater moving object position at the next time . 本发明结合水下运动体的运动特点,利用双级贝叶斯估计方法,首先自适应地估计水下运动体的速度大小和方向,再对水下运动体的位置进行估计,该方法贴合水下运动体的运动模型,能够较精确地估计出水下运动体的运动状态。 The present invention combines the characteristics of the motion of submerged bodies, Bayesian estimation using two-stage method, firstly adaptively estimated speed and direction of submerged bodies, and then the position estimate of submerged bodies, the bonding method motion model of submerged bodies, it is possible to more accurately estimate the state of motion of the submerged bodies.

[0031] 本发明的具体步骤如下: [0031] The specific steps of the present invention are as follows:

[0032] 步骤一:建立水下运动体的运动状态估计模型。 [0032] Step one: Create an underwater moving body motion estimation model.

[0033] 步骤二:利用观测量解算出前一时刻水下运动体的速度大小及方向。 [0033] Step 2: Using solution observables calculates a speed value and direction of submerged bodies of the previous time.

[0034]步骤三:利用下一时刻的观测信息更新贝叶斯估计参数。 [0034] Step Three: Update the parameter estimation using Bayesian observation information next time.

[0035] 步骤四:利用自适应贝叶斯运动估计方法,估计出水下运动体当前时刻的速度大小及方向。 [0035] Step Four: the adaptive motion estimation method Bayesian estimate the velocity magnitude and direction of the current time of submerged bodies.

[0036] 步骤五:利用水下运动体当前时刻位置和自适应贝叶斯的估计结果,通过无迹贝叶斯运动估计方法,得到下一时刻的水下运动体位置。 [0036] Step Five: estimation result using the current time and location of submerged bodies adaptive Bayes, Bayesian estimation method of motion without a trace, to obtain a next time underwater moving body position.

[0037] 本发明公布一种基于双贝叶斯估计的水下运动体运动状态估计方法,执行流程如附图1所示。 [0037] The invention discloses a method of motion estimation based on double submerged bodies Bayesian estimation, performing the processes shown in Figure 1. 具体的步骤说明如下: The specific steps are as follows:

[0038] 步骤一:建立水下运动体的运动状态估计模型。 [0038] Step one: Create an underwater moving body motion estimation model.

[0039] 状态估计的准确性与水下运动体模型息息相关。 [0039] state estimation accuracy of the model is closely related with the submerged bodies. 因此,要实现对水下运动体的跟踪和状态估计,首先要建立水下运动体的运动模型和观测模型。 Therefore, to achieve tracking of submerged bodies and state estimation, we must first establish the motion model and observation model of submerged bodies. 本专利在建模过程中将水下运动体作为一个整体,假设其为刚体,且所在水域流速为零。 This patent submerged bodies in the modeling process as a whole, which is assumed to be rigid, and where the water flow is zero. 具体建模步骤如下: Specific modeling steps as follows:

[0040] 步骤1.1建立水下运动体的观测模型 1.1 submerged bodies established observation model of [0040] Step

[0041] 对于水下运动体来说,某些要素比如水动力、推力等,无法通过观测获取。 [0041] For the submerged bodies, certain elements such as water, power, thrust, etc., can not be obtained through observation. 因此,需要根据实际情况对水下运动体的运动方程进行化简,本步骤给出水下运动体的运动方程和观测方程。 Thus, the need for simplification of the equation of motion of submerged bodies according to the actual situation, the present step and the observation equation gives the equation of motion of the submerged bodies.

[0042] 由于水下运动体的观测是通过声学传感器来完成的,因此得到的观测数据为极坐标下的数据,即水下运动体相对于观测点的距离R、平面偏向角A和垂直偏向角E。 [0042] Since the observations of submerged bodies is accomplished by the acoustic sensor, the observed data thus obtained polar coordinate data, i.e. submerged bodies with respect to the observation point distance R, and the plane vertical deflection deflection angle A angle E. 结合观测点坐标( x,y,z),可以得到观测方程: Binding observation point coordinates (x, y, z), observation equation can be obtained:

[0043] [0043]

Figure CN105513091AD00061

(1) (1)

[0044]其中,R是水下运动体相对于观测点的距离,A是水下运动体相对于观测点的平面偏向角,E是水下运动体相对于观测点的垂直偏向角,(ξ,τΐ,ζ)为水下运动体的世界坐标系坐标。 [0044] wherein, R is the distance of submerged bodies with respect to the observation point, A is submerged bodies with respect to the plane of the deflection angle of observation points, E is submerged bodies relative to the vertical deflection angle of observation points, (ξ , the world coordinate system coordinates τΐ, ζ) underwater moving body.

[0045]步骤1.2建立水下运动体的运动模型 1.2 motion model of submerged bodies [0045] Step

[0046] 水下运动体的运动模型的建立应当结合观测方程进行。 [0046] The motion model should be submerged bodies binding observation equation. 由观测方程中可知,对水下运动体的观测信息仅限于水下运动体与观测点自身的相对位置,通过观测方程(1),无法获得水下运动体自身的姿态角,因此,由于观测信息的不足,水下运动体的转动在建立运动方程时应被忽略。 Seen from the observation equation, the observation information of submerged bodies are limited to the relative position of the observation point of submerged bodies themselves, by observing equation (1) can not be obtained submerged bodies own attitude angle, therefore, due to the observed lack of rotation, underwater moving body information is ignored in the establishment of the equations of motion.

[0047] 但是由于水下运动体运动的连续性,其在某一时刻的位置信息不能完全反应出其运动情况,为了更全面地把握水下运动体的运动状态,将相邻两个时刻的观测信息结合起来,可以得到水下运动体在两个时刻之间速度大小和方向的平均值。 [0047] However, due to the continuity of the underwater moving body, the position information at a certain time can not fully reflect its movement, in order to more fully grasp motion state of submerged bodies, two adjacent time combine observation information can be obtained at the average speed of submerged bodies between the two moments of the magnitude and direction. 在双贝叶斯运动状态估计方法中,将速度作为整体看待,不将其投影至三个坐标轴。 In the dual motion Bayesian estimation process, viewed as a whole, the speed, it is not projected to three axes. 因此,记水下运动体的速度大小为V,速度方向与ηΕξ平面的夹角为Φ,速度方向与Εζ轴正方向夹角为Θ,根据以上需要, 可以得到水下运动体的运动方程为: Thus, the submerged bodies in mind the size of the speed V, the angle between the direction of velocity ηΕξ plane is [Phi], and the positive velocity direction angle of [Theta] axis direction Εζ, according to the above needs, the equations of motion can be obtained for submerged bodies :

[0048] [0048]

Figure CN105513091AD00071

(2) (2)

[0049] 式中,(ξ,η,ζ)为水下运动体的世界坐标系坐标。 [0049] In the formula, (ξ, η, ζ) of the world coordinate system of the submerged bodies.

[0050] 步骤1.3建立对水下运动体速度大小和方向进行估计的自适应贝叶斯估计器系统模型 1.3 establish underwater speed and direction body motion estimation [0050] Bayesian estimation step adaptive system model

[0051] 对水下运动体的速度大小和方向的估计,首先建立系统的状态转移方程和观测方程,确定其如下: [0051] The estimated speed and direction of movement of the underwater body, and the state transition equation established first observation equation system, which is determined as follows:

Figure CN105513091AD00072

[0052] [0052]

[0053] [0053]

[0054] [0054]

[0055] [0055]

[0056] [0056]

Figure CN105513091AD00081

[0057] 式中,T为采样时间片长度,(i)kSk时刻水下运动体的纵倾角,么为其角加速度,0k 为k时刻水下运动体的航向角,堯为其角加速度,vk为k时刻水下运动体的速度大小,1¾为其加速度,%*、分别为其角加速度和加速度的噪声,为均值不为零的白噪声,其均值分别为和£卜·,.)、.V为观测噪声。 [0057] In the formula, T is the sampling time chip length, the pitch angle of the submerged bodies (i) kSk time, what its angular acceleration, 0k heading angle is submerged bodies time k, Yao its angular acceleration, of VK velocity at time k the size of submerged bodies, 1¾ its acceleration,% *, respectively, and its angular acceleration noise, is not zero mean white noise, respectively, and the mean Bu £ ·,.) ,. V for the measurement noise. Q和R分别为过程噪声和观测噪声的均方差。 Q and R are the process noise and measurement noise variances are.

[0058] 步骤1.4建立对水下运动体位置进行估计的无迹贝叶斯估计器系统模型 1.4 establishing [0058] a step of estimating the position of submerged bodies unscented Bayesian estimation model system

[0059] 对水下运动体位置的估计,需要确定系统的状态转移方程和观测方程,确定其如下: [0059] The estimation of the position of submerged bodies, the system needs to determine the state transition equations and observation equations, it is determined as follows:

[0060] Xk+i = f(Xk, Φ ,0,v)+ffk (5) [0060] Xk + i = f (Xk, Φ, 0, v) + ffk (5)

[0061] yk = HXk+Vk (6) [0061] yk = HXk + Vk (6)

[0062] 其中: [0062] wherein:

[0063] [0063]

Figure CN105513091AD00082

[0064] 式中,T为采样时间片长度,Θ为水下运动体的航向角,φ为水下运动体的纵倾角,v 为水下运动体的速度大小。 [0064] In the formula, T is the sampling chip length, Θ is the angle of heading of submerged bodies, φ is the pitch angle of the submerged bodies, v is the velocity magnitude of submerged bodies. (ξ,η,ζ)为水下运动体的世界坐标系坐标。 (Ξ, η, ζ) is the world coordinate system of underwater moving body. w和v分别为过程噪声和观测噪声的均值,Q和R为其均方差。 w and v is the mean of the process noise and measurement noise, Q and R are as its variance.

[0065] 步骤二:利用观测量解算出前一时刻水下运动体的速度大小及方向。 [0065] Step 2: Using solution observables calculates a speed value and direction of submerged bodies of the previous time.

[0066] 为了实现对水下运动体的状态估计,需要解算水下运动体的速度大小及其方向, 速度方向包括水下运动体的纵倾角和偏航角。 [0066] In order to achieve the status of submerged bodies estimate, magnitude and speed resolver requires submerged bodies in the direction, the speed direction including the pitch angle and yaw angle of the submerged bodies. 本专利中的纵倾角和偏航角是指水下运动体的速度方向与水平面ηΕξ和Εζ轴正方向的夹角。 Pitch angle and yaw angle of the present patent refers to the angle between the direction of velocity of submerged bodies ηΕξ to the horizontal axis and the positive direction Εζ. 具体解算方程如下: DETAILED equation solver follows:

[0067] [0067]

Figure CN105513091AD00083

[0068] 假定在每个采样时间段内(采样时间段与传感器的采样间隔相同),水下运动体的速度大小和方向不变。 [0068] assumed that every sampling period (sampling interval of the sampling period and the same sensor), speed and direction of change of submerged bodies. 通过观测量解算出前一时刻水下运动体的速度及其方向。 Solutions by observables calculates a speed and direction of movement of the underwater body of the previous time.

[0069] 步骤三:利用下一时刻的观测信息更新贝叶斯估计参数 [0069] Step Three: observation information next time by using Bayesian estimation parameter updating

[0070] 在获得下一时刻的观测信息后,需要对贝叶斯估计的参数进行更新。 [0070] After obtaining observation information next time, Bayesian estimation parameters need to be updated. 具体包括自适应估计器和无迹估计器的参数更新。 Specifically includes updating the adaptive parameter estimation and unscented estimator.

[0071] 步骤3.1更新自适应估计器的参数,将水下运动体的角加速度和加速度的变化量视作系统噪声,使用噪声估计器进行估计。 [0071] Step 3.1 Update the adaptive parameter estimator, the submerged bodies and angular acceleration change amount of acceleration is regarded as the system noise using the noise estimator estimates. 具体方法如下: Specific methods are as follows:

[0072] 计算基于实际观测量的状态值戈,: [0072] Observations calculated based on the actual status values ​​Ge:

[0073] [0073]

Figure CN105513091AD00091

(9) (9)

[0074] 计算观测量的估计值和实际观测量的差值之: [0074] estimated value calculated for the measurements and the actual difference of observables:

[0075] Zk --Zk - HkXk kl -rk. .(10): [0075] Zk --Zk - HkXk kl -rk (10):..

[0076] 计算卡尔曼增益Kk: [0076] computing Kalman gain Kk:

[0077] [0077]

Figure CN105513091AD00092

(11) (11)

[0078] 斗管侦、古弟桂软u . [0078] bucket tube investigation, the ancient brother Gui soft u.

[0079] [0079]

Figure CN105513091AD00093

(12) (12)

[0080] 计算基于实际观测量的协方差Pk: [0080] covariance Pk is calculated based on the actual measurements with:

[0081] Pk=[I-KkHk]Pk,k-1 (13) [0081] Pk = [I-KkHk] Pk, k-1 (13)

[0082] 使用噪声估计器对水下运动体的角加速度和速度的变化量进行估计: [0082] using the noise estimator and the amount of change of angular velocity estimate is submerged bodies:

[0083] 计筧讨稈曝声询倌iU [0083] majority of counted discuss stalk sound exposure inquiry groom iU

[0084] [0084]

Figure CN105513091AD00094

(14) (14)

[0085] 计算过程噪声方差4: [0085] The noise variance computation 4:

[0086] 、…^ ^...... , (15) [0086], ... ^ ^ ......, (15)

Figure CN105513091AD00095

[0087] 计算观测噪声均值么: [0087] calculates the mean measurement noise:

[0088] [0088]

Figure CN105513091AD00096

(16) (16)

[0089] 计算观测噪声方差4: [0089] The observation noise variance is calculated 4:

[0090](17) [0090] (17)

[0091] 其中,遗忘因子: [0091] wherein the forgetting factor:

Figure CN105513091AD00097

[0092] [0092]

Figure CN105513091AD00098

(18) (18)

[0093]以上各式中,为转移矩阵,Hk为观测矩阵。 [0093] In the above formulas, as the transfer matrix, for the observation matrix Hk.

[0094] 步骤3.2利用观测信息更新无迹估计器的参数 [0094] Step 3.2 using observation information to update the parameter estimator unscented

[0095] 计算Sigma点对应的观测值Yk|k-1: [0095] The corresponding point calculation Sigma observations Yk | k-1:

[0096] 丨=/<名-丨) (丨9) [0096] Shu = / <name - Shu) (Shu 9)

[0097] 加权计算观测值的估计值兔卜丨: [0097] The estimated values ​​weighted observed values ​​rabbit Bu Shu:

[0098] [0098]

Figure CN105513091AD00101

(2U; (2U;

[0099] 计算观测估计值的协方差-〃: [0099] calculating the observed value of the estimated covariance -〃:

[0100] [0100]

Figure CN105513091AD00102

(21) (twenty one)

[0101 ]计算观测估计值与预测估计值的联合分布的协方差: [0101] covariance estimates and the observed joint distribution estimate predicted:

[0102] [0102]

Figure CN105513091AD00103

(.22) ί-0 ' (.22) ί-0 '

[0103] 计算卡尔曼增益Kk: [0103] computing Kalman gain Kk:

[0104] [,Κ 丨(23) [0104] [, Κ Shu (23)

[0105] 计算基于实际观测值的协方差Pk: [0105] covariance Pk is calculated based on the actual observed values ​​of:

[0106] [0106]

Figure CN105513091AD00104

(24) (twenty four)

[0107] 获得基于实际观测值的状态值毛:: [0107] obtained based on actual observations of the state gross value ::

[0108] [0108]

Figure CN105513091AD00105

(25) (25)

[0109] 式中,luSUT变换得到的sigma点,UT变换的过程在步骤五中有详细说明。 [0109] In the formula, luSUT transform the sigma points, UT conversion process in step 5 is described in detail. Ok, 为转移矩阵,Hk为观测矩阵。 Ok, for the transfer matrix, Hk is the observation matrix.

[0110] 通过以上过程即可完成贝叶斯估计器的参数更新。 [0110] to complete Bayesian estimator parameter update by the above process.

[0111] 步骤四:利用自适应贝叶斯运动估计方法,估计出水下运动体当前时刻的速度大小及方向。 [0111] Step Four: the adaptive motion estimation method Bayesian estimate the velocity magnitude and direction of the current time of submerged bodies.

[0112] 在步骤二中得到水下运动体的速度大小和方向,并在步骤三完成了贝叶斯估计的参数更新后,即可通过自适应贝叶斯运动估计方法中的估计方程,得到下一时刻水下运动体的速度大小和方向的估计值。 After [0112] to give speed and direction of submerged bodies in step two and step three complete Bayesian estimation parameter updating, the equation can be estimated by Bayesian estimation adaptive motion, to give estimate speed and direction of the next point of submerged bodies. 估计方程如下: Estimated equation is as follows:

[0113] [0113]

Figure CN105513091AD00106

(2〇) (2〇)

[0114] 式中,为预测值,4+,为参数更新后得到的前一时刻准确值,Φ为状态转移矩阵,Γ为扩维矩阵,u(Wk-〇为自适应估计得到的过程噪声的均值,即角速度和加速度的变化量。 [0114] In the formula, the predicted value, + 4, to a time before an accurate updated parameter value obtained, the state transition matrix [Phi], for the expansion Gamma] dimensional matrix, u (Wk-square adaptive noise estimation process obtained mean that the amount of change of angular velocity and acceleration.

[0115] 通过此方程得到在未来的一个时间片内水下运动体的速度大小和方向。 [0115] to give speed and direction of submerged bodies in the next time slice by this equation.

[0116] 步骤五:利用水下运动体当前时刻位置和自适应贝叶斯的估计结果,通过无迹贝叶斯运动估计方法,得到下一时刻的水下运动体位置。 [0116] Step Five: estimation result using the current time and location of submerged bodies adaptive Bayes, Bayesian estimation method of motion without a trace, to obtain a next time underwater moving body position.

[0117] 在步骤四中得到水下运动体未来时间片内的速度大小和方向后,通过无迹贝叶斯运动估计方法中的估计方程,得到下一时刻水下运动体的世界坐标系坐标。 After [0117] to give speed and direction of the next time slice submerged bodies in Step 4 without a trace by Bayesian estimation equation of motion estimation method to obtain the coordinates of the world coordinate system of the next time of submerged bodies .

[0118] 采用如下的无迹估计方法: [0118] The following unscented estimation method:

[0119] 计筧Siema 点: [0119] Total majority Siema points:

[0120] [0120]

Figure CN105513091AD00107

[0121 ]预测:首先计算每个Sigma点的函数值Xk+11 k,然后利用Sigma点的函数值进行加权, 得到函数预测值弋唓,最后计算估计协方差Pk+1|k。 [0121] Prediction: first calculating function value for each Xk Sigma point + 11 k, and then using the Sigma function value points are weighted to obtain the prediction function value Yi Che, and finally calculate the estimated covariance Pk + 1 | k.

[0122] xk+i|k = fk(xk) (28) [0122] xk + i | k = fk (xk) (28)

Figure CN105513091AD00111

[0125] 通过以上过程即可得到水下运动体位置的最终估计值。 The final estimate of [0125] to obtain the position of submerged bodies by the above process. [0126] 在得到新的观测量后,返回步骤二,可以继续进行估计。 [0126] After receiving the new observables returns to step two, may continue to be estimated.

[0123] [0123]

[0124] [0124]

Claims (1)

  1. I. 一种基于双贝叶斯估计的水下运动体运动状态估计方法,其特征在于,包括W下几个步骤: (1) 建立水下运动体的运动状态估计模型: (1.1) 建立水下运动体的观测模型: I. A method of estimating motion bis Bayesian estimation based on the submerged bodies, wherein W comprises the steps of: (1) establishment of the state of motion estimation models submerged bodies: (1.1) establishing water sports body under observation model:
    Figure CN105513091AC00021
    (1.2) 建立水下运动体的运动模型 (1.2) motion model of submerged bodies
    Figure CN105513091AC00022
    (1.3) 建立对水下运动体速度大小和方向进行估计的自适应贝叶斯估计器系统模型: (1.3) establish adaptive system model Bayesian estimation underwater speed and direction of motion estimation body:
    Figure CN105513091AC00023
    (1.4) 建立对水下运动体位置进行估计的无迹贝叶斯估计器系统模型: Xk+i = f(Xk, 4) ,0,v)+Wk yk = HXk+Vk (2) 利用观测量解算出前一时刻水下运动体的速度大小及方向: 通过观测量解算出前一时刻水下运动体的速度及其方向; (3) 利用下一时刻的观测信息更新贝叶斯估计参数: 在获得下一时刻的观测信息后,对贝叶斯估计的参数进行更新;分别进行自适应估计器和无迹估计器的相关参数更新; (3.1) 更新自适应估计器的参数,将水下运动体的角加速度和加速度的变化量视作系统噪声,使用噪声估计器进行估计; (3.2) 利用观测信息更新无迹估计器的参数; (4) 利用自适应贝叶斯运动估计方法,估计出水下运动体当前时刻的速度大小及方向; 通过自适应贝叶斯运动估计方法中的估计方程,得到下一时刻水下运动体的速度大小和方向的估计值; (5)利用水下运动体 (1.4) for establishing the position of submerged bodies estimate unscented Bayesian estimation system model: Xk + i = f (Xk, 4), 0, v) + Wk yk = HXk + Vk (2) using the observed Solutions for calculating the amount of velocity magnitude and direction of movement of the underwater body of a previous time: the velocity and observables solves for the direction of movement of the underwater body of a previous time; (3) using the observation information next time Bayesian estimation parameter updating : after obtaining the information on the next observation time, Bayesian estimation parameter update; adaptive parameters were estimated and the estimation is updated unscented; (3.1) updating the adaptive parameter estimator, water and the angular acceleration change amount of motion of the body regarded as system noise using the noise estimator estimates; (3.2) observed by using the updated parameter information without a trace estimator; (4) using a Bayesian adaptive motion estimation method, estimated velocity magnitude and direction of the current time of submerged bodies; estimation by adaptive Bayesian estimation equation of motion to obtain the estimated value of the speed and direction of the next time of submerged bodies; (5) using the underwater sports body 前时刻位置和自适应贝叶斯的估计结果,通过无迹贝叶斯运动估计方法,得到下一时刻的水下运动体位置。 Before the time position estimation result and an adaptive Bayesian, Bayesian estimation method of motion without a trace, to obtain a next time underwater moving body position.
CN 201510835168 2015-11-26 2015-11-26 Dual-Bayesian estimation-based motion state estimation method for underwater motion body CN105513091A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN 201510835168 CN105513091A (en) 2015-11-26 2015-11-26 Dual-Bayesian estimation-based motion state estimation method for underwater motion body

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN 201510835168 CN105513091A (en) 2015-11-26 2015-11-26 Dual-Bayesian estimation-based motion state estimation method for underwater motion body

Publications (1)

Publication Number Publication Date
CN105513091A true true CN105513091A (en) 2016-04-20

Family

ID=55721048

Family Applications (1)

Application Number Title Priority Date Filing Date
CN 201510835168 CN105513091A (en) 2015-11-26 2015-11-26 Dual-Bayesian estimation-based motion state estimation method for underwater motion body

Country Status (1)

Country Link
CN (1) CN105513091A (en)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6353679B1 (en) * 1998-11-03 2002-03-05 Compaq Computer Corporation Sample refinement method of multiple mode probability density estimation
CN102252672A (en) * 2011-04-28 2011-11-23 哈尔滨工程大学 Nonlinear filtering method for underwater navigation
CN103217175A (en) * 2013-04-10 2013-07-24 哈尔滨工程大学 Self-adaptive volume Kalman filtering method

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6353679B1 (en) * 1998-11-03 2002-03-05 Compaq Computer Corporation Sample refinement method of multiple mode probability density estimation
CN102252672A (en) * 2011-04-28 2011-11-23 哈尔滨工程大学 Nonlinear filtering method for underwater navigation
CN103217175A (en) * 2013-04-10 2013-07-24 哈尔滨工程大学 Self-adaptive volume Kalman filtering method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
周广宇 等: "基于双卡尔曼滤波及贝叶斯估计的微弱GPS 信号跟踪方法", 《上海交通大学学报》 *

Similar Documents

Publication Publication Date Title
CN101000507A (en) Method for moving robot simultanously positioning and map structuring at unknown environment
CN101893445A (en) Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition
US20090227266A1 (en) Location measurement method based on predictive filter
CN102722177A (en) Autonomous underwater vehicle (AUV) three-dimensional straight path tracking control method with PID (Piping and Instruments Diagram) feedback gain
CN102829777A (en) Integrated navigation system for autonomous underwater robot and method
CN102538781A (en) Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method
Indelman et al. Factor graph based incremental smoothing in inertial navigation systems
CN102096086A (en) Self-adaptive filtering method based on different measuring characteristics of GPS (Global Positioning System)/INS (Inertial Navigation System) integrated navigation system
CN102402225A (en) Method for realizing localization and map building of mobile robot at the same time
Kim et al. Unscented FastSLAM: A robust algorithm for the simultaneous localization and mapping problem
CN101943916A (en) Kalman filter prediction-based robot obstacle avoidance method
CN101819041A (en) Self-evolution ANFIS and UKF combined GPS/MEMS-INS integrated positioning error dynamic forecasting method
Rehder et al. Global pose estimation with limited GPS and long range visual odometry
JP2004251660A (en) Target tracking apparatus
CN101464134A (en) Vision measuring method for three-dimensional pose of spacing target
CN102980579A (en) Autonomous underwater vehicle autonomous navigation locating method
Gao et al. Random weighting method for multisensor data fusion
CN103063212A (en) Integrated navigation method based on non-linear mapping self-adaptive hybrid Kalman/H infinite filters
CN101982732A (en) Micro-satellite attitude determination method based on ESOQPF (estimar of quaternion particle filter ) and UKF (unscented kalman filter) master-slave filtering
Chiu et al. Robust vision-aided navigation using sliding-window factor graphs
JP2009128298A (en) Target tracking device
CN102749633A (en) Solution method for dynamic positioning of satellite navigation receiver
CN103697889A (en) Unmanned aerial vehicle self-navigation and positioning method based on multi-model distributed filtration
CN103278163A (en) Nonlinear-model-based SINS/DVL (strapdown inertial navigation system/doppler velocity log) integrated navigation method
CN103487047A (en) Improved particle filter-based mobile robot positioning method

Legal Events

Date Code Title Description
C06 Publication
C10 Entry into substantive examination