CN106354901A - Online identification method for mass property of carrier rocket and critical parameter of dynamics - Google Patents

Abstract

The invention relates to an online identification method for the mass property of carrier rocket and the critical parameter of dynamics. The method is based on the six degrees of freedom aviation dynamics equation and the unknown parameter dynamic equation. Through constructing the augmented state equation to realize a simultaneous estimate for state and parameter, the parameter vector and state vector to be identified is thus obtained. The greatest advantage of the present method is to estimate unknown parameters according to the output. When the system is changed, the mass property and the estimation result of the critical parameter of the dynamics can be accurately obtained, and can establish key technical foundation for online optimization and adjustment of the control parameters of carrier rocket. Meanwhile, the identification method can realize the simultaneous estimation of state and parameters online. The method has significances for reducing the conservativeness of rocket system deviations and for increasing the adaptability to malfunctions by rocket, and provides beneficial references for future rocket design.

Description

A kind of carrier rocket mass property and kinetics key parameter on-line identification method

Technical field

The present invention relates to a kind of carrier rocket mass property and kinetics key parameter on-line identification method, belong to control system System design field.

Background technology

At present China's carrier space vehicle control system design has, design low to system deviation bad adaptability, design level The too conservative problem of surplus, especially Control System Design are all carried out for nominal situation, the adaptability to malfunction Poor it is impossible to accurately estimate the crucial kinetic parameter in flight course, and realize the on-line tuning of control parameter accordingly to answer To various possible flight operating modes.Therefore, it is badly in need of carrying out carrier rocket key characteristic on-line parameter identification technical research, a side Face, is available for active service rocket and uses, be conducive to findding out system deviation, reduce the conservative of System Parameter Design, excavation type further Number potentiality, on the other hand, alternatively following rocket is realized the adjustment of control parameter on-line optimization and is established solid technical foundation, permissible Flight deviation and non-bust impact to aerial mission is greatly reduced.

The parameter that current carrier space vehicle control system is all based on nominal state carries out design, and these parameters are produced with kind Product, flight operating mode have certain difference, carry out envelope design often through wider deviation band, and Design of Attitude Control System is just It is to design a set of parameter being capable of the various operating mode of envelope, this certain conservative necessarily brought.During Project R＆D, warp Often occur due to deviation band wide, the problems such as Design of Attitude Control System is difficult.Although some parameters can carry out accurate measurement, or Test by large number of ground and accurately obtained, but, there are two large problems in this method, one is due to world diversity, Some parameters cannot accurately obtain, two be obtain accurate parameter cost excessive, and the distribution of every product is larger, is difficult to do To once and for all.Once additionally, breaking down in flight course, will be unable to realize control based on the control parameter of nominal state design System stability processed, needs to carry out accommodation according to practical flight characteristic to control parameter, and everything all be unable to do without acquisition Accurately carrier rocket mass property and kinetics key parameter.

Traditional carrier rocket key characteristic parameter identification technique is off-line identification technology, often in flight number of results After obtaining, parameter identification, this method one side simulation calculation amount are carried out by optimized algorithm according to flight result data Larger, on the other hand cannot realize on-line parameter identification.

Therefore, be badly in need of carrying out on-line parameter identification technique research, especially mass property and dynamicss parameter Line identification technique research, these parameters are the key characteristic parameters determining Control System Design.Because current rocket is not yet done These key parameters of method direct measurement (include quality, rotary inertia, barycenter, the pressure heart), need to study high-performance parameter identification calculation Method carries out on-line identification to above parameter.

Content of the invention

It is an object of the invention to overcoming the above-mentioned deficiency of prior art, provide a kind of carrier rocket mass property and power Learn key parameter on-line identification method, this discrimination method can be implemented in line states and parameter is estimated simultaneously, to reduction rocket system The conservative of system Deviation Design and raising rocket are significant to the adaptability of malfunction, can carry for following rocket design For beneficial reference.

The above-mentioned purpose of the present invention is mainly achieved by following technical solution:

A kind of carrier rocket mass property and kinetics key parameter on-line identification method, comprise the steps:

(1), according to carrier rocket six degree of freedom fight dynamics equation, state equation and measurement equation are obtained；

(2), described state equation and measurement equation are converted to State-Vector Equation；

(3), obtain augmented state side according to after the parameter vector equation combination of described State-Vector Equation and needs identification Journey；

(4), augmented state equation is carried out discretization, be converted into discrete iteration equation；

(5), according to described discrete iteration equation, obtain the parameter vector of needs identification using filtering algorithm, obtain simultaneously State vector.

State in above-mentioned carrier rocket mass property and kinetics key parameter on-line identification method, in step (1) Equation is as follows:

$\left(\begin{array}{c}{\stackrel{\·}{x}}_a\\ {\stackrel{\·}{y}}_a\\ {\stackrel{\·}{z}}_a\end{array}\right)=\left(\begin{array}{c}{v}_{ax}\\ v_{ay}\\ v_{az}\end{array}\right)---\left(8\right)$

Measurement equation is as follows:

$\left(\begin{array}{c}{x}_a^{\′}\\ y_a^{\′}\\ z_a^{\′}\end{array}\right)=\left(\begin{array}{c}{x}_a\\ y_a\\ z_a\end{array}\right)+\left(\begin{array}{c}{v}_{xx}\\ v_{xy}\\ v_{xz}\end{array}\right)---\left(12\right)$

$\left(\begin{array}{c}{v}_{ax}^{\′}\\ v_{ay}^{\′}\\ v_{az}^{\′}\end{array}\right)=\left(\begin{array}{c}{v}_{ax}\\ v_{ay}\\ v_{az}\end{array}\right)+\left(\begin{array}{c}{v}_{vx}\\ v_{vy}\\ v_{vz}\end{array}\right)---\left(13\right)$

$\left(\begin{array}{c}{\mathrm{\ω}}_{x_1}^{\′}\\ {\mathrm{\ω}}_{y_1}^{\′}\\ {\mathrm{\ω}}_{z_1}^{\′}\end{array}\right)=\left(\begin{array}{c}{\mathrm{\ω}}_{x_1}\\ {\mathrm{\ω}}_{y_1}\\ {\mathrm{\ω}}_{z_1}\end{array}\right)+\left(\begin{array}{c}{v}_{\mathrm{\ω}x}\\ v_{\mathrm{\ω}y}\\ v_{\mathrm{\ω}z}\end{array}\right)---\left(15\right)$

Wherein: w_iFor state-noise；v_iFor measurement noise, x_a,y_a,z_aFor the coordinate under launch inertial coordinate system, v_ax,v_ay, v_azFor the speed in x, y, z direction under launch inertial coordinate system,ψ, γ are the attitude angle under launch inertial coordinate system,For under launch vehicle coordinate system around the attitude angular velocity of x, y, z axle；δ_ψ,δ_γFor electromotor pivot angle；g_ax,g_ay,g_azFor The gravitational acceleration in x, y, z direction under launch inertial coordinate system；x_rFor motor power application point and rocket theory cusp away from From z_rDistance for motor power application point and rocket axis；M is Rocket mass, j_x1,j_y1,j_z1For under launch vehicle coordinate system around The rocket rotary inertia of x, y, z axle, x_zFor the distance of rocket barycenter and rocket theory cusp, p is separate unit motor power.

In above-mentioned carrier rocket mass property and kinetics key parameter on-line identification method, in step (2) state to Amount equation is as follows:

$\stackrel{\·}{x}\left(t\right)=f(x,\mathrm{\λ},t)+w_x\left(t\right)---\left(16\right)$

Z (t)=h (x, t)+v (t) (17)

Wherein:Shape for system State vector, λ (t)=(λ₁λ₂)^tFor the unknown parameter vector of system, λ₁=m, λ₂=p,For the output vector of system, h (x, t) =i_12×12X (t), i_12×12Be with unknown parameter vector incoherent 12 dimension * 12 dimension unit matrixs, f (x, λ, t) be with unknown The related nonlinear function vector of parameter vector, expression formula is as follows:

Wherein: f_x(λ₁),f_y(λ₁),f_z(λ₁),f_xz(λ₁) be and λ₁Related known function, w_xT () is made an uproar for system mode Sound vector；V (t) is systematic survey noise vector.

In above-mentioned carrier rocket mass property and kinetics key parameter on-line identification method, system mode noise vector w_xT () average is zero, variance matrix isWhite Gaussian noise；Described systematic survey noise vector v (t) Average is zero, and variance matrix is r=e [v (t) v^t(t)] white Gaussian noise.

In above-mentioned carrier rocket mass property and kinetics key parameter on-line identification method, according to shape in step (3) It is as follows that the parameter vector equation of state vector equation and needs identification obtains augmented state equation concrete grammar:

Need the parameter vector equation of identification as follows:

$\stackrel{\·}{\mathrm{\λ}}\left(t\right)=g\left(t\right)+w_{\mathrm{\λ}}\left(t\right)---\left(18\right)$

Wherein: g (t)=(f_m(t)f_p(t))^tFor the known function under various operating modes, w_λT () is parametric noise vector, f_m T () changes over function, f for quality_pT () changes over function for thrust；

In conjunction with (16) and (18) formula, the unknown parameter vector extensions in system are the state vector of augmented system, obtain The state vector of augmented system is:

$x\left(t\right)=\left[\begin{array}{c}x\left(t\right)\\ \mathrm{\λ}\left(t\right)\end{array}\right]---\left(19\right)$

Thus obtaining augmented state equation is:

$\stackrel{\·}{x}\left(t\right)=\left[\begin{array}{c}\stackrel{\·}{x}\left(t\right)\\ \stackrel{\·}{\mathrm{\λ}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}f(x,\mathrm{\λ},t)\\ g\left(t\right)\end{array}\right]+\left[\begin{array}{c}{w}_x\left(t\right)\\ w_{\mathrm{\λ}}\left(t\right)\end{array}\right].---\left(20\right)$

In above-mentioned carrier rocket mass property and kinetics key parameter on-line identification method, parametric noise vector w_λ T () average is zero, variance matrix isWhite Gaussian noise.

In above-mentioned carrier rocket mass property and kinetics key parameter on-line identification method, by augmentation in step (4) The concrete grammar of state equation discretization is as follows:

Augmented state equation discretization can be obtained:

$\begin{array}{c}x(k+1)=\left[\begin{array}{c}x(k+1)\\ \mathrm{\λ}(k+1)\end{array}\right]=\left[\begin{array}{c}x\left(k\right)+f\left(x\right(k),\mathrm{\λ}(k\left)\right)t\\ \mathrm{\λ}\left(k\right)+g\left(k\right)t\end{array}\right]+\left[\begin{array}{c}{w}_x\left(k\right)\\ w_{\mathrm{\λ}}\left(k\right)\end{array}\right]\\ =\left[\begin{array}{c}x\left(k\right)\\ \mathrm{\λ}\left(k\right)\end{array}\right]+\left[\begin{array}{c}f\left(x\right(k),\mathrm{\λ}(k\left)\right)\\ g\left(k\right)\end{array}\right]t+\left[\begin{array}{c}{w}_x\left(k\right)\\ w_{\mathrm{\λ}}\left(k\right)\end{array}\right]=x\left(k\right)+f\left(x\right(k\left)\right)t+w\left(k\right)\end{array}---\left(21\right)$

Wherein: t be the sampling period, k=1,2 ...,

$f\left(x\right(k\left)\right)=\left[\begin{array}{c}f\left(x\right(k),\mathrm{\λ}(k\left)\right)\\ g\left(k\right)\end{array}\right],w\left(k\right)=\left[\begin{array}{c}{w}_x\left(k\right)\\ w_{\mathrm{\λ}}\left(k\right)\end{array}\right],$

Z (k)=h (x (k))+v (k) (22)

Wherein: f (x (k), λ (k)) is that (x (k) and λ (k) t) is substituted into after discretization and obtains f by x, λ；H (x (k)) be h (x, T) after discretization, x (k) is substituted into and obtain；G (k) obtains for g (t) discretization.

In above-mentioned carrier rocket mass property and kinetics key parameter on-line identification method, using filter in step (5) Ripple algorithm obtain needs identification parameter vector and state vector concrete grammar as follows:

(1), initialized according to kth -1 step result of calculationp_λ(k | k), p_x(k | k), v (k), wherein K=1,2 ...；

(2), φ (k), ψ (k) are calculated；

$\mathrm{\φ}\left(k\right)=i_{12\×12}+a\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)t$

$\mathrm{\ψ}\left(k\right)=b\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)t$

Wherein:

$a\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)=\frac{\∂f(x,\mathrm{\λ},t)}{\∂x}{|}_{x=\hat{x}\left(k\right|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}\left(k\right|k)}$

$b\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k\left|k\right))=\frac{\∂f(x,\mathrm{\λ},t)}{\∂\mathrm{\λ}}{|}_{x=\hat{x}\left(k\right|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}\left(k\right|k)};$

(3), calculate kth+1 step parameter optimum prediction:

$\widehat{\mathrm{\λ}}(k+1|k)=\widehat{\mathrm{\λ}}\left(k\right|k)+g\left(k\right)t---\left(23\right)$

Calculate kth+1 step state optimization to predict simultaneously:

$\hat{x}(k+1|k)=\hat{x}\left(k\right|k)+f\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)t---\left(24\right)$

Wherein:For f, (x, λ, t) discretization willWithSubstitution obtains；

(4), calculate kth+1 step parameter prediction error covariance matrix:

p_λ(k+1 | k)=p_λ(k|k)+q_λ(25)

Calculating kth+1 step status predication error covariance matrix simultaneously:

p_x(k+1 | k)=φ (k) p_x(k|k)φ^t(k)+ψ(k)p_λ(k|k)ψ^t(k)+q_x(26)

(5), calculate h (k+1), φ (k+1), ψ (k+1):

$\mathrm{\φ}(k+1)=i+a\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)t---\left(27\right)$

$\mathrm{\ψ}(k+1)=b\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)t---\left(28\right)$

$h(k+1)=\frac{\∂h(x,t)}{\∂x}{|}_{x=\hat{x}(k+1|k)}---\left(29\right)$

Wherein: $a\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)=\frac{\∂f(x,\mathrm{\λ},t)}{\∂x}{|}_{x=\hat{x}(k+1|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}(k+1|k)},$

$b\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)=\frac{\∂f(x,\mathrm{\λ},t)}{\∂\mathrm{\λ}}{|}_{x=\hat{x}(k+1|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}(k+1|k)};$

(6), calculate state optimization gain matrix:

k_x(k+1)=p_x(k+1|k)h^t(k+1)[h(k+1)p_x(k+1|k)h^t(k+1)+r]^-1(30)

(7), calculate weighting matrix:

U (k+1)=φ (k+1) v (k)+ψ (k+1) (31)

S (k+1)=h (k+1) u (k+1) (32)

V (k+1)=u (k+1)-k_x(k+1)s(k+1) (33)

K (k+1)=k_x(k+1)+v(k+1)k_λ(k+1) (34)

(8), calculating parameter optimum gain matrix:

$\begin{array}{c}{k}_{\mathrm{\λ}}(k+1)=p_{\mathrm{\λ}}(k+1|k)s^t(k+1)\[s(k+1)p_{\mathrm{\λ}}(k+1|k)s^t(k+1)\\ +h(k+1)p_x(k+1|k)h^t(k+1)+r{\]}^{-1}\end{array}---\left(35\right)$

(9), calculating parameter optimal estimation value:

$\widehat{\mathrm{\λ}}(k+1|k+1)=\widehat{\mathrm{\λ}}(k+1|k)+k_{\mathrm{\λ}}(k+1)\[z\left(k\right)-h\left(\hat{x}\right(k+1|k\left)\right)\]---\left(36\right)$

Wherein: z (k) represents the measured value of kth step；

Calculating state optimization estimated value simultaneously:

$\hat{x}(k+1|k+1)=\hat{x}(k+1|k)+k(k+1)\[z(k+1)-h\left(\hat{x}\right(k+1|k\left)\right)\]---\left(37\right)$

Wherein:Will for h (x, t) discretizationSubstitution obtains；

(10), calculating parameter filtering error variance matrix:

p_λ(k+1 | k+1)=[i-k_λ(k+1)s(k+1)]p_λ(k+1|k) (38)

Calculating state filtering error covariance matrix simultaneously:

p_x(k+1 | k+1)=[i-k_x(k+1)h(k+1)]p_x(k+1|k) (39)

(11), the discreet value of kth+1 step that above-mentioned kth step is calculatedp_λ(k+1|k +1)、p_x(k+1 | k+1), v (k+1) initializationp_λ(k | k), p_x(k | k), v (k), wherein k=1, 2 ..., return to step (2).

The present invention compared with prior art has the advantages that

(1), the carrier rocket mass property based on state and Decoupled estimation proposed by the present invention and kinetics are crucial On-line parameter identification method, the method can be implemented in line states and parameter is estimated simultaneously, to reduction rocket system Deviation Design Conservative and improve rocket the adaptability of malfunction be significant, can be that the design of following rocket provides beneficial ginseng Examine；

(2), the carrier rocket mass property based on state and Decoupled estimation proposed by the present invention and kinetics are crucial On-line parameter identification method, the method is based on six degree of freedom fight dynamics equation and unknown parameter dynamical equation, by construction Augmented state equation is realized estimating to while state and parameter, thus the parameter vector needing identification and state can be obtained simultaneously Vector, the maximum advantage of this method is according to output, unknown parameter can be estimated, when system changes, The estimated result of mass property and kinetics key parameter can accurately be obtained, can adjust for carrier rocket control parameter on-line optimization The whole technical foundation establishing key；

(3), achievement in research of the present invention can be applicable to, in China's various carrier space vehicle control system development, also apply be applicable to each In the design of class flight control, additionally, the inventive method has generality, also may extend to general control system In design, practical.

Brief description

Fig. 1 is carrier rocket mass property of the present invention and kinetics key parameter on-line identification method flow diagram；

Fig. 2 is x direction location estimation result in the embodiment of the present invention；

Fig. 3 is y direction location estimation result in the embodiment of the present invention；

Fig. 4 is z direction location estimation result in the embodiment of the present invention；

Fig. 5 is pitch attitude angular estimation result in the embodiment of the present invention；

Fig. 6 is yaw-position angular estimation result in the embodiment of the present invention；

Fig. 7 is roll attitude angular estimation result in the embodiment of the present invention；

Fig. 8 is quality estimation results in the embodiment of the present invention；

Fig. 9 is thrust estimated result in the embodiment of the present invention.

Specific embodiment

The present invention is described in further detail with specific embodiment below in conjunction with the accompanying drawings:

The present invention, before carrying out carrier rocket mass property and dynamicss on-line parameter identification, first has to set up respectively Kinetics relation between individual characteristic quantity, this is the key realizing parameter identification.Then estimate with Decoupled using based on state The expanded Kalman filtration algorithm of meter, recognizes to unknown mass property and dynamicss parameter.Ask for simplifying the analysis The difficulty of topic, taking extra-atmospheric flight section as a example carries out the technical research of correlation.

It is illustrated in figure 1 carrier rocket mass property of the present invention and kinetics key parameter on-line identification method flow diagram； Initially set up six degree of freedom fight dynamics equation and unknown parameter dynamical equation, then unknown parameter identification problem is converted into The problem that state is estimated with Decoupled, the Kalman filtering algorithm finally by state and parameter estimation obtains unknown matter Flow characteristic and the estimated result of kinetics key parameter, concrete methods of realizing is as follows:

The first step, sets up fight dynamics equation.

According to basic flight dynamics principle, be not added with deriving directly to the rocket flight kinetics side set out under used system Journey:

$\left(\begin{array}{c}{\stackrel{\·}{x}}_a\\ {\stackrel{\·}{y}}_a\\ {\stackrel{\·}{z}}_a\end{array}\right)=\left(\begin{array}{c}{v}_{ax}\\ v_{ay}\\ v_{za}\end{array}\right)---\left(1\right)$

$\left(\begin{array}{c}{\stackrel{\·}{v}}_{ax}\\ {\stackrel{\·}{v}}_{ay}\\ {\stackrel{\·}{v}}_{az}\end{array}\right)=\left(\begin{array}{c}{a}_{ax}\\ a_{ay}\\ a_{az}\end{array}\right)+\left(\begin{array}{c}{g}_{ax}\\ g_{ay}\\ g_{az}\end{array}\right)---\left(2\right)$

Wherein,

Rocket rotation around center of mass kinetics equation under rocket body system:

Mass property relational expression:

$\begin{array}{c}{x}_z=f_{xz}\left(m\right)\\ j_{x1}=f_x\left(m\right)\\ j_{y1}=f_y\left(m\right)\\ j_{z1}=f_z\left(m\right)\end{array}---\left(5\right)$

There is following dynamical equation in Rocket mass in flight course:

$\stackrel{\·}{m}=f_m\left(t\right)---\left(6\right)$

There is following dynamical equation in rocket thrust in flight course:

$\stackrel{\·}{p}=f_p\left(t\right)---\left(7\right)$

Wherein: x_a,y_a,z_aFor the coordinate under launch inertial coordinate system, v_ax,v_ay,v_azFor x, y, z under launch inertial coordinate system The speed in direction, a_ax,a_ay,a_azFor the apparent acceleration in x, y, z direction under launch inertial coordinate system, a_x1,a_y1,a_z1For launch vehicle coordinate The apparent acceleration in the lower x, y, z direction of system,ψ, γ are the attitude angle in x, y, z direction under launch inertial coordinate system,For The angular velocity in x, y, z direction under launch vehicle coordinate system, all can be calculated by navigation and obtain；δ_ψ,δ_γFor electromotor pivot angle, permissible Obtained by control instruction；g_ax,g_ay,g_azGravitational acceleration for x, y, z direction under launch inertial coordinate system；x_rFor electromotor Thrust point and the distance of rocket theory cusp, z_rDistance for motor power application point and rocket axis；M is rocket matter Amount, j_x1,j_y1,j_z1For the rocket rotary inertia in x, y, z direction under launch vehicle coordinate system, x_zFor rocket barycenter and rocket theory cusp Distance, p be separate unit motor power, be need identification amount；f_mT () changes over function, f for quality_pT () is to push away Power changes over function, f_xz(m), f_x(m), f_y(m), f_zM () is the function with mass change, above-mentioned function is known letter Number.

There is certain noise and perturbation in view of state and measurement parameter, above-mentioned equation can be written as following form:

State equation:

$\left(\begin{array}{c}{\stackrel{\·}{x}}_a\\ {\stackrel{\·}{y}}_a\\ {\stackrel{\·}{z}}_a\end{array}\right)=\left(\begin{array}{c}{v}_{ax}\\ v_{ay}\\ v_{az}\end{array}\right)---\left(8\right)$

Measurement equation:

$\left(\begin{array}{c}{x}_a^{\′}\\ y_a^{\′}\\ z_a^{\′}\end{array}\right)=\left(\begin{array}{c}{x}_a\\ y_a\\ z_a\end{array}\right)+\left(\begin{array}{c}{v}_{xx}\\ v_{xy}\\ v_{xz}\end{array}\right)---\left(12\right)$

$\left(\begin{array}{c}{v}_{ax}^{\′}\\ v_{ay}^{\′}\\ v_{az}^{\′}\end{array}\right)=\left(\begin{array}{c}{v}_{ax}\\ v_{ay}\\ v_{az}\end{array}\right)+\left(\begin{array}{c}{v}_{vx}\\ v_{vy}\\ v_{vz}\end{array}\right)---\left(13\right)$

$\left(\begin{array}{c}{\mathrm{\ω}}_{x_1}^{\′}\\ {\mathrm{\ω}}_{y_1}^{\′}\\ {\mathrm{\ω}}_{z_1}^{\′}\end{array}\right)=\left(\begin{array}{c}{\mathrm{\ω}}_{x_1}\\ {\mathrm{\ω}}_{y_1}\\ {\mathrm{\ω}}_{z_1}\end{array}\right)+\left(\begin{array}{c}{v}_{\mathrm{\ω}x}\\ v_{\mathrm{\ω}y}\\ v_{\mathrm{\ω}z}\end{array}\right)---\left(15\right)$

Wherein, w_iFor state-noise, it is zero mean Gaussian white noise under normal flight operating mode, is non-under fault condition Zero-mean coloured noise, can be modeled by fault mode；v_iFor measurement noise, it is zero mean Gaussian white noise.

Second step, is converted to state space equation.

For the ease of carrier rocket mass property and kinetics key parameter on-line identification are realized using filtering algorithm, will be upper State kinetics equation and be rewritten as the form of following state space equation and (directly obtain formula by formula (8), (9), (10), (11) (16), directly obtain formula (17) by formula (12), (13), (14), (15):

$\stackrel{\·}{x}\left(t\right)=f(x,\mathrm{\λ},t)+w_x\left(t\right)---\left(16\right)$

Z (t)=h (x, t)+v (t) (17)

Wherein:State for system Vector, λ (t)=(λ₁λ₂)^tFor the unknown parameter vector of system, λ₁=m, λ₂=p,For the output vector of system, h (x, t) =i_12×12X (t), i_12×12Be with unknown parameter vector incoherent 12 dimension * 12 dimension unit matrixs, f (x, λ, t) be with unknown The related nonlinear function vector of parameter vector, expression formula is as follows:

Wherein: f_x(λ₁),f_y(λ₁),f_z(λ₁),f_xz(λ₁) be and λ₁Related known function, such as in the present embodiment For:

f_x(λ₁)=5.7978 × 10^-3λ₁+3.5436×10⁴

f_y(λ₁)=2.523 × 10^-9λ₁ ³-5.764×10^-4λ₁ ²+55.3322λ₁+9.28155×10⁵

f_z(λ₁)=2.523 × 10^-9λ₁ ³-5.764×10^-4λ₁ ²+55.3322λ₁+9.28155×10⁵

f_xz(λ₁)=1.000 × 10^-14λ₁ ³-2.7956×10^-9λ₁ ²+2.5586×10^-4λ₁+11.5529

w_x(t) be system mode noise vector, be average be zero, variance matrix isWhite Gaussian Noise, v (t) be systematic survey noise vector, be average be zero, variance matrix be r=e [v (t) v^t(t)] white Gaussian noise.

3rd step, sets up augmented system equation.

In above-mentioned state space equation, λ is the parameter needing to be recognized.Accurately to obtain unknown parameter λ, to need first The dynamic law of λ to be obtained, is carried out augmentation to original system, then using filtering algorithm, it is recognized.

λ (t) can be by its change procedure of following differential equation:

$\stackrel{\·}{\mathrm{\λ}}\left(t\right)=g\left(t\right)+w_{\mathrm{\λ}}\left(t\right)---\left(18\right)$

Wherein: g (t)=(f_m(t)f_p(t))^tFor the known function under various operating modes, w_λT () is parametric noise vector, be Average is zero, and variance matrix isWhite Gaussian noise.

f_mT () changes over function, f for quality_pT () changes over function for thrust；

In conjunction with (16) and (18) formula, the unknown parameter in system can be expanded to the state of augmented system, obtain augmentation shape State is

$x\left(t\right)=\left[\begin{array}{c}x\left(t\right)\\ \mathrm{\λ}\left(t\right)\end{array}\right]---\left(19\right)$

Thus obtaining augmented state equation is

$\stackrel{\·}{x}\left(t\right)=\left[\begin{array}{c}\stackrel{\·}{x}\left(t\right)\\ \stackrel{\·}{\mathrm{\λ}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}f(x,\mathrm{\λ},t)\\ g\left(t\right)\end{array}\right]+\left[\begin{array}{c}{w}_x\left(t\right)\\ w_{\mathrm{\λ}}\left(t\right)\end{array}\right]---\left(20\right)$

Therefore, the essence that system unknown parameter is estimated is exactly state unknown parameter being expanded to system, then using each Plant the estimation that EKF method carries out augmented state, thus obtaining the estimated value of unknown parameter.

4th step, by augmented system model discretization.

Augmented system discretization can be obtained:

$\begin{array}{c}x(k+1)=\left[\begin{array}{c}x(k+1)\\ \mathrm{\λ}(k+1)\end{array}\right]=\left[\begin{array}{c}x\left(k\right)+f\left(x\right(k),\mathrm{\λ}(k\left)\right)t\\ \mathrm{\λ}\left(k\right)+g\left(k\right)t\end{array}\right]+\left[\begin{array}{c}{w}_x\left(k\right)\\ w_{\mathrm{\λ}}\left(k\right)\end{array}\right]\\ =\left[\begin{array}{c}x\left(k\right)\\ \mathrm{\λ}\left(k\right)\end{array}\right]+\left[\begin{array}{c}f\left(x\right(k),\mathrm{\λ}(k\left)\right)\\ g\left(k\right)\end{array}\right]t+\left[\begin{array}{c}{w}_x\left(k\right)\\ w_{\mathrm{\λ}}\left(k\right)\end{array}\right]=x\left(k\right)+f\left(x\right(k\left)\right)t+w\left(k\right)\end{array}---\left(21\right)$

Wherein: t be the sampling period, k=1,2 ...,

$f\left(x\right(k\left)\right)=\left[\begin{array}{c}f\left(x\right(k),\mathrm{\λ}(k\left)\right)\\ g\left(k\right)\end{array}\right],w\left(k\right)=\left[\begin{array}{c}{w}_x\left(k\right)\\ w_{\mathrm{\λ}}\left(k\right)\end{array}\right].$

Z (k)=h (x (k))+v (k) (22)

Wherein: f (x (k), λ (k)) is that (x (k) and λ (k) t) is substituted into after discretization and obtains f by x, λ；H (x (k)) be h (x, T) x (k) is substituted into and obtains by discretization；G (k) obtains for g (t) discretization.

So far, as long as parameter spread to be estimated is augmented system state, more just can be obtained using spreading kalman algorithm The estimated value of unknown parameter.

5th step, is estimated to unknown parameter using the expanded Kalman filtration algorithm that state is estimated with Decoupled.

The maximum difficulty of the problems referred to above is due to being augmented system state by parameter spread to be estimated, thus leading to system Amount of calculation increase, this for carrier rocket such high order system be totally unfavorable it is therefore necessary to adopt state and parameter The filtering algorithm that decoupling is estimated is solved.It not only can unknown parameter in real-time estimation system, and adjustment is estimated accordingly Meter state, but also by two parallelism wave filters, state and parameter can be carried out with decoupling and estimate, in terms of reaching minimizing system The purpose of calculation amount.

The filtering algorithm based on state with Decoupled estimated is given below, when starting to calculate, p_λ (k | k), p_x(k | k), the initial value of v (k) is previously given, when calculating afterwards, is calculated using the result of calculation of previous step as next step Initial value, below by kth step calculate as a example,

(1) initialized according to kth -1 step result of calculationp_λ(k | k), p_x(k | k), v (k)；K=1, 2,...

(2) calculating φ (k), ψ (k):

$\mathrm{\φ}\left(k\right)=i+a\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)t$

$\mathrm{\ψ}\left(k\right)=b\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)t$

Wherein:

$a\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)=\frac{\∂f(x,\mathrm{\λ},t)}{\∂x}{|}_{x=\hat{x}\left(k\right|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}\left(k\right|k)}$

$b\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)=\frac{\∂f(x,\mathrm{\λ},t)}{\∂\mathrm{\λ}}{|}_{x=\hat{x}\left(k\right|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}\left(k\right|k)}$

(3) calculate kth+1 step parameter optimum prediction:

$\widehat{\mathrm{\λ}}(k+1|k)=\widehat{\mathrm{\λ}}\left(k\right|k)+g\left(k\right)t---\left(23\right)$

Calculate kth+1 step state optimization to predict simultaneously:

$\hat{x}(k+1|k)=\hat{x}\left(k\right|k)+f\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)t---\left(24\right)$ Wherein:For (x, λ, t) discretization will for fWithSubstitution obtains；

(4) calculate kth+1 step parameter prediction error covariance matrix:

p_λ(k+1 | k)=p_λ(k|k)+q_λ(25)

Calculating kth+1 step status predication error covariance matrix simultaneously:

p_x(k+1 | k)=φ (k) p_x(k|k)φ^t(k)+ψ(k)p_λ(k|k)ψ^t(k)+q_x(26)

(5) calculate h (k+1), φ (k+1), ψ (k+1):

$\mathrm{\φ}(k+1)=i+a\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)t---\left(27\right)$

$\mathrm{\ψ}(k+1)=b\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)t---\left(28\right)$

$h(k+1)=\frac{\∂h(x,t)}{\∂x}{|}_{x=\hat{x}(k+1|k)}---\left(29\right)$

Wherein: $a\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)=\frac{\∂f(x,\mathrm{\λ},t)}{\∂x}{|}_{x=\hat{x}(k+1|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}(k+1|k)}$

$b\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)=\frac{\∂f(x,\mathrm{\λ},t)}{\∂\mathrm{\λ}}{|}_{x=\hat{x}(k+1|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}(k+1|k)}.$

(6) calculate state optimization gain matrix:

k_x(k+1)=p_x(k+1|k)h^t(k+1)[h(k+1)p_x(k+1|k)h^t(k+1)+r]^-1(30)

(7) calculate weighting matrix:

U (k+1)=φ (k+1) v (k)+ψ (k+1) (31)

S (k+1)=h (k+1) u (k+1) (32)

V (k+1)=u (k+1)-k_x(k+1)s(k+1) (33)

K (k+1)=k_x(k+1)+v(k+1)k_λ(k+1) (34)

(8) calculating parameter optimum gain matrix:

$\begin{array}{c}{k}_{\mathrm{\λ}}(k+1)=p_{\mathrm{\λ}}(k+1|k)s^t(k+1)\[s(k+1)p_{\mathrm{\λ}}(k+1|k)s^t(k+1)\\ +h(k+1)p_x(k+1|k)h^t(k+1)+r{\]}^{-1}\end{array}---\left(35\right)$

(9) calculating parameter optimal estimation value:

$\widehat{\mathrm{\λ}}(k+1|k+1)=\widehat{\mathrm{\λ}}(k+1|k)+k_{\mathrm{\λ}}(k+1)\[z\left(k\right)-h\left(\hat{x}\right(k+1|k\left)\right)\]---\left(36\right)$

Wherein: z (k) represents the measured value of kth step,

Including angular velocity, angle, position, speed measured value；

Calculating state optimization estimated value simultaneously:

$\hat{x}(k+1|k+1)=\hat{x}(k+1|k)+k(k+1)\[z(k+1)-h\left(\hat{x}\right(k+1|k\left)\right)\]---\left(37\right)$

Wherein:Will for h (x, t) discretizationSubstitution obtains；

(10) calculating parameter filtering error variance matrix:

p_λ(k+1 | k+1)=[i-k_λ(k+1)s(k+1)]p_λ(k+1|k) (38)

Calculating state filtering error covariance matrix simultaneously:

p_x(k+1 | k+1)=[i-k_x(k+1)h(k+1)]p_x(k+1|k) (39)

(11), complete kth step to calculate, the discreet value of kth+1 step that above-mentioned kth step is calculatedp_λ(k+1|k+1)、p_x(k+1 | k+1), v (k+1) initialization p_λ (k | k), p_x(k | k), v (k), wherein k=1,2 ..., return to step (2), start the calculating of kth+1 step.

$\widehat{\mathrm{\λ}}(k+1|k+1)\→\widehat{\mathrm{\λ}}\left(k\right|k),\hat{x}(k+1|k+1)\→\hat{x}\left(k\right|k),$

p_λ(k+1|k+1)→p_λ(k | k), p_x(k+1|k+1)→p_x(k | k), v (k+1) → v (k).

Finally give state estimation and parameter estimation result, that is, need the parameter vector recognizingAnd state vectorAs the design considerationss carrying out the adjustment of control parameter on-line optimization.

Above-mentioned parameter discrimination method is a kind of double estimation filtering methods of state and parameter estimation in theory in fact, The state and parameter of unknown dynamical system is alternately estimated, this method uses model to estimate signal, and using estimation Signal removes correction model.

Embodiment 1

Adopt following initial value according to formula (23)-(39), you can realize carrier rocket mass property and kinetics key parameter On-line identification, concrete identification result is shown in Fig. 2-Fig. 9.

p_λ(0 | 0)=100i_2×2, p_x(0 | 0)=100i_12×12, v (0)=i_12×12.Its In, x₀For state of flight initial value, λ₀For estimating initial parameter values.

If Fig. 2 is x direction location estimation result in the embodiment of the present invention, Fig. 3 is that in the embodiment of the present invention, y direction position is estimated Meter result；Fig. 4 is z direction location estimation result in the embodiment of the present invention；Fig. 5 is pitch attitude angular estimation in the embodiment of the present invention Result；Fig. 6 is yaw-position angular estimation result in the embodiment of the present invention；Fig. 7 is roll attitude angular estimation in the embodiment of the present invention Result；Fig. 8 is quality estimation results in the embodiment of the present invention；Fig. 9 is thrust estimated result in the embodiment of the present invention.From identification knot Fruit understands, the carrier rocket mass property estimated with Decoupled based on state being proposed and kinetics key parameter are distinguished online Know algorithm and can realize state of flight and the accurate estimation of unknown parameter, its estimated accuracy can meet design objective demand.

The above, the only optimal specific embodiment of the present invention, but protection scope of the present invention is not limited thereto, Any those familiar with the art the invention discloses technical scope in, the change or replacement that can readily occur in, All should be included within the scope of the present invention.

The content not being described in detail in description of the invention belongs to the known technology of professional and technical personnel in the field.

Claims (8)

1. a kind of carrier rocket mass property and kinetics key parameter on-line identification method it is characterised in that: include following walking Rapid:

(1), according to carrier rocket six degree of freedom fight dynamics equation, state equation and measurement equation are obtained；

(2), described state equation and measurement equation are converted to State-Vector Equation；

(3), obtain augmented state equation according to after the parameter vector equation combination of described State-Vector Equation and needs identification；

(4), augmented state equation is carried out discretization, be converted into discrete iteration equation；

(5), according to described discrete iteration equation, obtain the parameter vector of needs identification using filtering algorithm, obtain state simultaneously Vector.

2. a kind of carrier rocket mass property according to claim 1 and kinetics key parameter on-line identification method, its It is characterised by: the state equation in described step (1) is as follows:

$\left(\begin{array}{c}{\stackrel{\·}{x}}_a\\ {\stackrel{\·}{y}}_a\\ {\stackrel{\·}{z}}_a\end{array}\right)=\left(\begin{array}{c}{v}_{ax}\\ v_{ay}\\ v_{az}\end{array}\right)---\left(8\right)$

Measurement equation is as follows:

$\left(\begin{array}{c}{z}_a^{\′}\\ z_a^{\′}\\ z_a^{\′}\end{array}\right)=\left(\begin{array}{c}{x}_{ax}\\ y_{ay}\\ z_{az}\end{array}\right)+\left(\begin{array}{c}{v}_{xx}\\ v_{xy}\\ v_{xz}\end{array}\right)---\left(12\right)$

$\left(\begin{array}{c}{v}_{ax}^{\′}\\ v_{ay}^{\′}\\ v_{az}^{\′}\end{array}\right)=\left(\begin{array}{c}{v}_{ax}\\ v_{ay}\\ v_{az}\end{array}\right)+\left(\begin{array}{c}{v}_{vx}\\ v_{vy}\\ v_{vz}\end{array}\right)---\left(13\right)$

$\left(\begin{array}{c}{\mathrm{\ω}}_{x_1}^{\′}\\ {\mathrm{\ω}}_{y_1}^{\′}\\ {\mathrm{\ω}}_{z_1}^{\′}\end{array}\right)=\left(\begin{array}{c}{\mathrm{\ω}}_{x_1}\\ {\mathrm{\ω}}_{y_1}\\ {\mathrm{\ω}}_{z_1}\end{array}\right)+\left(\begin{array}{c}{v}_{\mathrm{\ω}x}\\ v_{\mathrm{\ω}y}\\ v_{\mathrm{\ω}z}\end{array}\right)---\left(15\right)$

Wherein: w_iFor state-noise；v_iFor measurement noise, x_a,y_a,z_aFor the coordinate under launch inertial coordinate system, v_ax,v_ay,v_azFor The speed in x, y, z direction under launch inertial coordinate system,ψ, γ are the attitude angle under launch inertial coordinate system,For arrow Around the attitude angular velocity of x, y, z axle under body coordinate system；δ_ψ,δ_γFor electromotor pivot angle；g_ax,g_ay,g_azFor launch inertial coordinate system The gravitational acceleration in lower x, y, z direction；x_rFor the distance of motor power application point and rocket theory cusp, z_rPush away for electromotor Point of force application and the distance of rocket axis；M is Rocket mass, j_x1,j_y1,j_z1For turning around the rocket of x, y, z axle under launch vehicle coordinate system Dynamic inertia, x_zFor the distance of rocket barycenter and rocket theory cusp, p is separate unit motor power.

3. a kind of carrier rocket mass property according to claim 1 and 2 and kinetics key parameter on-line identification method, It is characterized in that: in described step (2), State-Vector Equation is as follows:

$\stackrel{\·}{x}\left(t\right)=f(x,\mathrm{\λ},t)+w_x\left(t\right)---\left(16\right)$

Z (t)=h (x, t)+v (t) (17)

Wherein:For system state to Amount, λ (t)=(λ₁λ₂)^tFor the unknown parameter vector of system, λ₁=m, λ₂=p,For the output vector of system, h (x, t) =i_12×12X (t), i_12×12Be with unknown parameter vector incoherent 12 dimension * 12 dimension unit matrixs, f (x, λ, t) be with unknown The related nonlinear function vector of parameter vector, expression formula is as follows:

Wherein: f_x(λ₁),f_y(λ₁),f_z(λ₁),f_xz(λ₁) be and λ₁Related known function, w_x(t) be system mode noise to Amount；V (t) is systematic survey noise vector.

4. a kind of carrier rocket mass property according to claim 3 and kinetics key parameter on-line identification method, its It is characterised by: described system mode noise vector w_xT () average is zero, variance matrix isWhite Gaussian Noise；Described systematic survey noise vector v (t) average is zero, and variance matrix is r=e [v (t) v^t(t)] white Gaussian noise.

5. a kind of carrier rocket mass property according to claim 3 and kinetics key parameter on-line identification method, its It is characterised by: in described step (3), augmented state side is obtained according to the parameter vector equation of State-Vector Equation and needs identification Journey concrete grammar is as follows:

Need the parameter vector equation of identification as follows:

$\stackrel{\·}{\mathrm{\λ}}\left(t\right)=g\left(t\right)+w_{\mathrm{\λ}}\left(t\right)---\left(18\right)$

Wherein: g (t)=(f_m(t) f_p(t))^tFor the known function under various operating modes, w_λT () is parametric noise vector, f_mT () is Quality changes over function, f_pT () changes over function for thrust；

In conjunction with (16) and (18) formula, the unknown parameter vector extensions in system are the state vector of augmented system, obtain augmentation The state vector of system is:

$x\left(t\right)=\left[\begin{array}{c}x\left(t\right)\\ \mathrm{\λ}\left(t\right)\end{array}\right]---\left(19\right)$

Thus obtaining augmented state equation is:

$\stackrel{\·}{x}\left(t\right)=\left[\begin{array}{c}\stackrel{\·}{x}\left(t\right)\\ \stackrel{\·}{\mathrm{\λ}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}f(x,\mathrm{\λ},t)\\ g\left(t\right)\end{array}\right]+\left[\begin{array}{c}{w}_x\left(t\right)\\ w_{\mathrm{\λ}}\left(t\right)\end{array}\right].---\left(20\right)$

6. a kind of carrier rocket mass property according to claim 5 and kinetics key parameter on-line identification method, its It is characterised by: described parametric noise vector w_λT () average is zero, variance matrix isWhite Gaussian noise.

7. a kind of carrier rocket mass property according to claim 5 and kinetics key parameter on-line identification method, its It is characterised by: will be as follows for the concrete grammar of augmented state equation discretization in described step (4):

Augmented state equation discretization can be obtained:

$\begin{array}{c}x(k+1)=\left[\begin{array}{c}x(k+1)\\ \mathrm{\λ}(k+1)\end{array}\right]=\left[\begin{array}{c}x\left(k\right)+f(x\left(k\right),\mathrm{\λ}\left(k\right))t\\ \mathrm{\λ}\left(k\right)+g\left(k\right)t\end{array}\right]+\left[\begin{array}{c}{w}_x\left(k\right)\\ w_{\mathrm{\λ}}\left(k\right)\end{array}\right]\\ =\left[\begin{array}{c}x\left(k\right)\\ \mathrm{\λ}\left(k\right)\end{array}\right]+\left[\begin{array}{c}f(x\left(k\right),\mathrm{\λ}\left(k\right))\\ g\left(k\right)\end{array}\right]t+\left[\begin{array}{c}{w}_x\left(k\right)\\ w_{\mathrm{\λ}}\left(k\right)\end{array}\right]=x\left(k\right)+f\left(x\right(k\left)\right)t+w\left(k\right)\end{array}---\left(21\right)$

Wherein: t be the sampling period, k=1,2 ...,

$f\left(x\right(k\left)\right)=\left[\begin{array}{c}f(x\left(k\right),\mathrm{\λ}\left(k\right))\\ g\left(k\right)\end{array}\right],w\left(k\right)=\left[\begin{array}{c}{w}_x\left(k\right)\\ w_{\mathrm{\λ}}\left(k\right)\end{array}\right],$

Z (k)=h (x (k))+v (k) (22)

Wherein: f (x (k), λ (k)) is that (x (k) and λ (k) t) is substituted into after discretization and obtains f by x, λ；H (x (k)) be h (x, t) from After dispersion, x (k) is substituted into and obtain；G (k) obtains for g (t) discretization.

8. a kind of carrier rocket mass property according to claim 7 and kinetics key parameter on-line identification method, its It is characterised by: in described step (5), obtain the parameter vector of needs identification and the concrete grammar of state vector using filtering algorithm As follows:

(1), initialized according to kth -1 step result of calculationp_λ(k | k), p_x(k | k), v (k), wherein k= 1,2,...；

(2), φ (k), ψ (k) are calculated；

$\mathrm{\φ}\left(k\right)=i_{12\×12}+a\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)t$

$\mathrm{\ψ}\left(k\right)=b\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)t$

Wherein:

$a\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)=\frac{\∂f(x,\mathrm{\λ},t)}{\∂x}{|}_{x=\hat{x}\left(k\right|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}\left(k\right|k)},$

$b\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)=\frac{\∂f(x,\mathrm{\λ},t)}{\∂\mathrm{\λ}}{|}_{x=\hat{x}\left(k\right|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}\left(k\right|k)};$

(3), calculate kth+1 step parameter optimum prediction:

$\widehat{\mathrm{\λ}}(k+1|k)=\widehat{\mathrm{\λ}}\left(k\right|k)+g\left(k\right)t---\left(23\right)$

Calculate kth+1 step state optimization to predict simultaneously:

$\hat{x}(k+1|k)=\hat{x}\left(k\right|k)+f\left(\hat{x}\right(k|k),\widehat{\mathrm{\λ}}(k|k\left)\right)t---\left(24\right)$

Wherein:For f, (x, λ, t) discretization willWithSubstitution obtains；

(4), calculate kth+1 step parameter prediction error covariance matrix:

p_λ(k+1 | k)=p_λ(k|k)+q_λ(25)

Calculating kth+1 step status predication error covariance matrix simultaneously:

p_x(k+1 | k)=φ (k) p_x(k|k)φ^t(k)+ψ(k)p_λ(k|k)ψ^t(k)+q_x(26)

(5), calculate h (k+1), φ (k+1), ψ (k+1):

$\mathrm{\φ}(k+1)=i+a\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)t---\left(27\right)$

$\mathrm{\ψ}(k+1)=b\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)t---\left(28\right)$

$h(k+1)=\frac{\∂h(x,t)}{\∂x}{|}_{x=\hat{x}(k+1|k)}---\left(29\right)$

Wherein:

$b\left(\hat{x}\right(k+1|k),\widehat{\mathrm{\λ}}(k+1|k\left)\right)=\frac{\∂f(x,\mathrm{\λ},t)}{\∂\mathrm{\λ}}{|}_{x=\hat{x}(k+1|k),\mathrm{\λ}=\widehat{\mathrm{\λ}}(k+1|k)};$

(6), calculate state optimization gain matrix:

k_x(k+1)=p_x(k+1|k)h^t(k+1)[h(k+1)p_x(k+1|k)h^t(k+1)+r]^-1(30)

(7), calculate weighting matrix:

U (k+1)=φ (k+1) v (k)+ψ (k+1) (31)

S (k+1)=h (k+1) u (k+1) (32)

V (k+1)=u (k+1)-k_x(k+1)s(k+1) (33)

K (k+1)=k_x(k+1)+v(k+1)k_λ(k+1) (34)

(8), calculating parameter optimum gain matrix:

$\begin{array}{c}{k}_{\mathrm{\λ}}(k+1)=p_{\mathrm{\λ}}(k+1|k)s^t(k+1)\[s(k+1)p_{\mathrm{\λ}}(k+1|k)s^t(k+1)\\ +h(k+1)p_x(k+1|k)h^t(k+1)+r{\]}^{-1}\end{array}---\left(35\right)$

(9), calculating parameter optimal estimation value:

$\widehat{\mathrm{\λ}}(k+1|k+1)=\widehat{\mathrm{\λ}}(k+1|k)+k_{\mathrm{\λ}}(k+1)\[z\left(k\right)-h\left(\hat{x}\right(k+1|k\left)\right)\]---\left(36\right)$

Wherein: z (k) represents the measured value of kth step；

Calculating state optimization estimated value simultaneously:

$\hat{x}(k+1|k+1)=\hat{x}(k+1|k)+k(k+1)\[z(k+1)-h\left(\hat{x}\right(k+1|k\left)\right)\]---\left(37\right)$

Wherein:Will for h (x, t) discretizationSubstitution obtains；

(10), calculating parameter filtering error variance matrix:

p_λ(k+1 | k+1)=[i-k_λ(k+1)s(k+1)]p_λ(k+1|k) (38)

Calculating state filtering error covariance matrix simultaneously:

p_x(k+1 | k+1)=[i-k_x(k+1)h(k+1)]p_x(k+1|k) (39)

(11), the discreet value of kth+1 step that above-mentioned kth step is calculatedp_λ(k+1|k+1)、 p_x(k+1 | k+1), v (k+1) initializationp_λ(k | k), p_x(k | k), v (k), wherein k=1,2 ..., return Return step (2).