CN105446352B

CN105446352B - A kind of proportional navigation law recognizes filtering method

Info

Publication number: CN105446352B
Application number: CN201510822999.2A
Authority: CN
Inventors: 邹昕光; 周荻; 朱蕊蘋
Original assignee: Harbin Institute of Technology
Current assignee: Harbin Institute of Technology
Priority date: 2015-11-23
Filing date: 2015-11-23
Publication date: 2018-04-24
Anticipated expiration: 2035-11-23
Also published as: CN105446352A

Abstract

A kind of proportional navigation law recognizes filtering method, and the present invention relates to proportional navigation law to recognize filtering method.The present invention is to cause the problem of model is inaccurate, and estimated accuracy is low to solve known to existing MMAE wave filters hypothesis PN Guidance Law navigation constants and do not consider pursuer controllers saturated conditions.The present invention initially sets up PN Guidance Laws and saturation motion model in pitching and the state equation of the plane of yaw；Then the state-transition matrix of computing system and design Kalman filter equation；Then the posterior probability of pursuer current times PN Guidance Law motion model and saturation motion model is calculated；At the time of finally calculating the switching of pursuer motion models according to posterior probability, at this moment otherwise the preceding estimated result using saturation motion model Kalman filter equation uses the estimated result of PN Guidance Law motion model Kalman filter equations.The present invention is applied to space industry.

Description

A kind of proportional navigation law recognizes filtering method

Technical field

The present invention relates to proportional navigation law to recognize filtering method.

Background technology

In exoatmosphere ballistic missile penetration application, in order to improve the penetration ability of trajectory, ballistic missile can be big Defending missile and its synchronized accompanying flying are discharged outside gas-bearing formation.Controlled by Guidance Law when defending missile finds interception guided missile and touched with it Hit, so as to improve the penetraton probability of ballistic missile.The optimal evasive strategy of ballistic missile and the optimal guidance law of defending missile Design is all assumed that known to the Guidance Law of interception guided missile.Although the motor-driven of interception guided missile can be regarded based on sliding formwork control Guidance Law Unknown external disturbance is handled, but if the acceleration of interception guided missile can be estimated, can greatly improve sliding mode guidance Performance.Therefore the identification problem of research interception guided missile Guidance Law has actual meaning.

Shaferman et al. exists《JOURNAL OF GUIDANCE CONTROL AND DYNAMICS》Magazine 2010 year Article " the Cooperative Multiple-Model Adaptive Guidance for an Aircraft of 6 phases Devised in Defending Missile " a kind of winged based on MMAE (Multiple-Model Adaptive Estimator) Machine Initiative Defense cooperative guidance is restrained.Assuming that interception guided missile uses proportional guidance (PN) Guidance Law, enhancing proportional guidance (APN) guidance One kind in rule or optimal guidance law (OGL), using multi-model self-adapting filter (MMAE) to the Guidance Law of interception guided missile into Row identification.But the work has two.1) this method assumes that interception guided missile employs known Guidance Law constant N.This limit The scope of application of the MMAE is made.In actual scene, the navigation constant of interception guided missile Guidance Law is unknown.2) this method does not have There is the situation for considering interception guided missile actuator saturation.Under normal circumstances, can be mended in guidance phases initial stages, interception guided missile due to quick Repay initial alignment error and temporarily enter actuator saturation state, controller exits saturation state after the error is compensated. Without considering the situation of actuator saturation, it will make it that MMAE filter models are inaccurate, estimated accuracy is low.

The content of the invention

The purpose of the present invention is to solve problems with：1) existing method assumes that interception guided missile employs known guidance Constant N is restrained, limits the scope of application of MMAE.And in actual scene, the navigation constant of interception guided missile Guidance Law is unknown； 2) interception guided missile actuator is likely to occur saturated conditions in guidance initial time.Existing method does not account for above actual conditions, So that the problem of MMAE filter models are inaccurate, and estimated accuracy is low, leads in view of the above problems, the present invention proposes a kind of ratio Draw Guidance Law identification filtering method.

Above-mentioned goal of the invention is achieved through the following technical solutions：

Step 1：Pursuer has two kinds of motion models：Saturation motion model and controller during controller saturation is unsaturated When PN Guidance Law motion models；State equation, the PN Guidance Laws that PN Guidance Laws motion model is established in pitch plane move mould Type is moved in state equation, the controller saturation motion model of the plane of yaw in the state equation and controller saturation of pitch plane State equation of the model in the plane of yaw；Evader is ballistic missile；Pursuer is interception guided missile；PN Guidance Laws are led for ratio Draw Guidance Law；

Step 2：Moved according to state equation, PN Guidance Law of the PN Guidance Laws motion model of step 1 in pitch plane Model is transported in state equation, the controller saturation motion model of the plane of yaw in the state equation and controller saturation of pitch plane Movable model is in the state equation of the plane of yaw, state-transition matrix of the design PN Guidance Laws motion model in pitch plane, PN systems State-transition matrix, the controller saturation motion model that rule motion model is led in the plane of yaw shift square in the state of pitch plane Battle array, controller saturation motion model the plane of yaw state-transition matrix；

Step 3：According to state-transition matrix of the PN Guidance Laws motion model of step 2 in pitch plane, PN Guidance Laws Motion model the state-transition matrix of the plane of yaw, controller saturation motion model pitch plane state-transition matrix, Controller saturation motion model the plane of yaw state-transition matrix, design PN Guidance Law motion models in pitch plane Kalman filter equation, PN Guidance Laws motion model are moved in the Kalman filter equation of the plane of yaw, controller saturation Model pitch plane Kalman filter equation and controller saturation motion model the plane of yaw Kalman filter Equation, Kalman filter equation are Kalman filter equation；

Step 4：According to Kalman filter equation of the PN Guidance Laws motion model of step 3 in pitch plane, PN systems Lead rule motion model the Kalman filter equation of the plane of yaw, controller saturation motion model pitch plane Kalman Filter equation and controller saturation motion model in the Kalman filter equation of the plane of yaw, calculate pursuer it is current when Carve the posterior probability of PN Guidance Law motion models and the posterior probability of controller saturation motion model；

Step 5：The posterior probability of the pursuer current time PN Guidance Law motion models obtained according to step 4 and full With the posterior probability of motion model, calculate pursuer from controller saturation when saturation motion model be switched to controller and do not satisfy With when PN Guidance Law motion models at the time of；Saturation motion model when pursuer is from controller saturation is switched to control Before at the time of PN Guidance Law motion models when device is unsaturated using controller saturation motion model pitch plane Kalman The estimated result of filter equation and controller saturation motion model in the Kalman filter equation of the plane of yaw；Otherwise use PN Guidance Laws motion model pitch plane Kalman filter equation and PN Guidance Law motion models in the plane of yaw The estimated result of Kalman filter equation.

Invention effect

Proportional navigation law identification wave filter proposed by the present invention employs multiple model filtering method.Devise pin respectively To the extended Kalman filter of PN Guidance Laws motion model and saturation motion model.Wherein PN Guidance Laws motion model is by PN systems The navigation constant of rule is led as state to be estimated, so as to solve the unknown actual conditions of PN Guidance Law navigation constants.It is above-mentioned Two class extended Kalman filters are run parallel, the probability of two models are calculated by Bayesian inference frame, so as to pick out The motion model that interception guided missile uses, and then estimate the acceleration and PN Guidance Law navigation constants of interception guided missile.This is solved Interception guided missile guide initial stage its controller may saturation the problem of.Filtering method proposed by the present invention is only adopted relative to traditional With the Extended Kalman filter method of PN Guidance Law models, its estimated accuracy improves 40-50% or so.

The possibility that the pursuer controllers during guidance produce saturation in the early stage is considered, has used multi-model filter Wave method recognizes pursuer Guidance Laws, to the navigation constant under PN Guidance Laws and in pitch plane and the plane of yaw Acceleration is estimated.More accurately Guidance Law can be recognized.In an embodiment of the present invention, if MMAE wave filters pair The mean square error that pursuer acceleration estimations produce is Var_MMAE；Whole process produces pursuer acceleration estimations using PN Guidance Laws Raw mean square error is Var_PN.If α=Var_MMAE/Var_PN.As 0 ＜ α ＜ 1, MMAE wave filters to pursuer acceleration estimations compared with Illustrate whole more accurate to pursuer acceleration estimations using PN Guidance Laws；α is smaller, illustrates to guide using PN relative to whole process For rule for pursuer acceleration estimations, MMAE is more accurate to pursuer acceleration estimations.

When evader without it is motor-driven when, in pursuer saturation stages, to pursuer acceleration in Evader inertial coodinate systems y The component a of axis_yEstimation when α=0.59；To pursuer acceleration Evader inertial coodinate system z-axis component a_zEstimation when α=0.56.

When evader carry out constant value it is motor-driven when, in pursuer saturation stages, to pursuer acceleration a_yEstimation when α= 0.55；To pursuer acceleration a_zEstimation when α=0.62.

When evader carries out sinusoidal motor-driven, in pursuer saturation stages, to pursuer acceleration a_yEstimation when α= 0.56；To pursuer acceleration a_zEstimation when α=0.60.

Brief description of the drawings

Fig. 1 is pursuer and evader relative motion relation schematic diagrames, and E is seats of the evader under evader inertial systems Mark, P are coordinates of the pursuer under evader inertial systems, q_pεFor sight pitch angle, q_pβFor sight yaw angle, r evader To the relative distance of pursuer, LOS₀For the initial sight of evader to pursuer；

Fig. 2 for Evader constant value it is motor-driven in the case of to Pursuer pitch plane navigation constants N_εEstimated result figure；

Fig. 3 for Evader constant value it is motor-driven in the case of to Pursuer plane of yaw navigation constants N_βEstimated result figure；

Fig. 4 recognizes wave filter using PN Guidance Laws Kalman in the case of being moved for Evader is inorganic and exists to Pursuer acceleration The component a of Evader inertial coodinate system y-axis_yEstimated result figure, g are acceleration of gravity；

Fig. 5 recognizes wave filter using PN Guidance Laws Kalman in the case of being moved for Evader is inorganic and exists to Pursuer acceleration The component a of Evader inertial coodinate system z-axis_zEstimated result figure, g are acceleration of gravity；

Fig. 6 uses MMAE wave filters to Pursuer acceleration a in the case of being moved for Evader is inorganic_yEstimated result figure；

Fig. 7 uses MMAE wave filters to Pursuer acceleration a in the case of being moved for Evader is inorganic_zEstimated result figure；

Fig. 8 for Evader constant value it is motor-driven in the case of using PN Guidance Laws Kalman recognize wave filter to Pursuer acceleration a_yEstimated result figure；

Fig. 9 for Evader constant value it is motor-driven in the case of using PN Guidance Laws Kalman recognize wave filter to Pursuer acceleration a_zEstimated result figure；

Figure 10 for Evader constant value it is motor-driven in the case of using MMAE wave filters to Pursuer acceleration a_yEstimated result figure；

Figure 11 for Evader constant value it is motor-driven in the case of using MMAE wave filters to Pursuer acceleration a_zEstimated result figure；

Figure 12 uses PN Guidance Laws Kalman to recognize wave filter to Pursuer acceleration in the case of being moved for Evader sine mechanisms a_yEstimated result figure；

Figure 13 uses PN Guidance Laws Kalman to recognize wave filter to Pursuer acceleration in the case of being moved for Evader sine mechanisms a_zEstimated result figure；

Figure 14 uses MMAE wave filters to Pursuer acceleration a in the case of being moved for Evader sine mechanisms_yEstimated result figure；

Figure 15 uses MMAE wave filters to Pursuer acceleration a in the case of being moved for Evader sine mechanisms_zEstimated result figure.

Embodiment

Embodiment one：Present embodiment a kind of proportional navigation law identification filtering method, specifically according to Prepared by following steps：

Step 2：Moved according to state equation, PN Guidance Law of the PN Guidance Laws motion model of step 1 in pitch plane Model is transported in state equation, the controller saturation motion model of the plane of yaw in the state equation and controller saturation of pitch plane Movable model calculates state-transition matrix of the PN Guidance Laws motion model in pitch plane, PN systems in the state equation of the plane of yaw State-transition matrix, the controller saturation motion model that rule motion model is led in the plane of yaw shift square in the state of pitch plane Battle array, controller saturation motion model the plane of yaw state-transition matrix；

Embodiment two：The present embodiment is different from the first embodiment in that：Pursuer in the step 1 There are two kinds of motion models：PN Guidance Law motion models when saturation motion model and controller during controller saturation are unsaturated； PN Guidance Laws motion model is established in the state equation of pitch plane, PN Guidance Law motion models in the state side of the plane of yaw Journey, controller saturation motion model pitch plane state equation and controller saturation motion model the plane of yaw state Equation；Detailed process is：

In missile breakthrough scene, anti-ballistic missile of dashing forward will escape the interception guided missile of other side's missile defense systems transmitting Intercept, be escape side, be known as evader, and its relevant all physical quantity all contains subscript e；Interception guided missile is the side of chasing, and is claimed All contain subscript p for pursuer, and its relevant all physical quantity；Assuming that the Guidance and control of interception guided missile can be decoupled as longitudinal direction Plane and lateral plane.Fig. 1 depicts the relative motion relation of pursuer and evader in pitch plane.

Evader LOS coordinates system defines；LOS coordinate system o ' x₄y₄z₄Origin o ' is located at the target seeker centre of gyration, o ' x₄Axis Consistent with target-guided missile sight, it is just o ' y to be directed toward target by the target seeker centre of gyration₄Axle position is in including o ' x₄The plummet face of axis It is interior, with o ' x₄Axis is vertical, points up as just, o ' z₄Axis is determined by the right-hand rule；And the terminal guidance inertial coodinate system of evader is determined Justice is the evader LOS coordinates system o of guidance initial time₀x₀y₀z₀；

Pursuer needs to eliminate alignment error initial stage in guidance, this is likely to result in pursuer controllers and enters saturation shape State, after error concealment, pursuer controllers can exit saturation state；Therefore pursuer has two kinds of motion models：Controller PN Guidance Law motion models when saturation motion model and controller during saturation are unsaturated；Two kinds of fortune of pursuer are given below Move the model in pitch plane and the plane of yaw；

The PN Guidance Laws equation of motion modeling of pursuer, r_xIt is evader and pursuer relative positions in evader inertia It is the component under x-axis；r_yFor component of the evader and pursuer relative positions under evader inertial system y-axis；r_zFor evader With component of the pursuer relative positions under evader inertial system z-axis；Evader is ballistic missile, and pursuer leads for interception Bullet；

r_x=x_p-x_e r_y=y_p-y_e r_z=z_p-z_e (1)

In formula, [x_p,y_p,z_p]^T[x_e,y_e,z_e]^TIt is the position of pursuer and evader under evader inertial systems respectively Put, T is transposition, x_pFor positions of the pursuer under evader inertial system x-axis, y_pIt is pursuer under evader inertial system y-axis Position, z_pFor positions of the pursuer under evader inertial system z-axis, x_eFor positions of the evader under evader inertial system x-axis Put, y_eFor positions of the evader under evader inertial system y-axis, z_eFor positions of the evader under evader inertial system z-axis；

PN Guidance Laws motion model is in the state equation of pitch plane

PN Guidance Laws motion model is in the state equation of the plane of yaw

In formula, v_xFor component of the evader and pursuer relative velocities under evader inertial system x-axis, v_yFor evader and Component of the pursuer relative velocities under evader inertial system y-axis, v_zIt is evader and pursuer relative velocities in evader Component under inertial system z-axis；a_pxFor component of the acceleration under evader inertial system x-axis of pursuer, a_pyFor pursuer's Component of the acceleration under evader inertial system y-axis, a_pzFor component of the acceleration under evader inertial system z-axis of pursuer； a_exFor component of the acceleration under evader inertial system x-axis of evader, a_eyFor evader acceleration in evader inertial systems Component under y-axis, a_ezFor component of the acceleration under evader inertial system z-axis of evader；N_εIt is pursuer in pitch plane Navigation constant；N_βIt is navigation constants of the pursuer in the plane of yaw；τ is time constant,For r_xFirst derivative,For r_y First derivative,For r_zFirst derivative,For v_xFirst derivative,For v_yFirst derivative,For v_zFirst derivative,For a_pxFirst derivative,For a_pyFirst derivative,For a_pzFirst derivative,For N_εFirst derivative,For N_β First derivative；

Assuming that evader can obtain positions of the pursuer under inertial system, and then calculate opposite itself (i.e. evader) Position；The measurement equation for calculating the opposite position of itself is

H=[r_x,r_y,r_z]^T (4)

Controller saturation motion model is in the state equation of pitch plane

Controller saturation motion model is in the state equation of the plane of yaw

In formula, d₁And d₂It is the occupied state of controller saturation motion model；Purpose be in order to PN Guidance Law motion models Dimension be consistent.

Other steps and parameter are identical with embodiment one.

Embodiment three：The present embodiment is different from the first and the second embodiment in that：Root in the step 2 According to step 1 PN Guidance Laws motion model the state equation of pitch plane, PN Guidance Laws motion model the plane of yaw shape State equation, controller saturation motion model pitch plane state equation and controller saturation motion model in the plane of yaw State equation, calculates PN Guidance Laws motion model and is being yawed in state-transition matrix, the PN Guidance Laws motion model of pitch plane The state-transition matrix of plane, controller saturation motion model are moved in the state-transition matrix of pitch plane, controller saturation State-transition matrix of the model in the plane of yaw；Detailed process is：

PN Guidance Laws motion model is in the state-transition matrix of pitch plane

In formula, T is measurement period, and unit is the second；

PN Guidance Laws motion model is in the state-transition matrix of the plane of yaw

Controller saturation motion model is in the state-transition matrix of pitch plane

Controller saturation motion model is in the state-transition matrix of the plane of yaw

Other steps and parameter are the same as one or two specific embodiments.

Embodiment four：Unlike one of present embodiment and embodiment one to three：The step 3 The middle PN Guidance Laws motion model according to step 2 is being yawed in state-transition matrix, the PN Guidance Laws motion model of pitch plane The state-transition matrix of plane, controller saturation motion model are moved in the state-transition matrix of pitch plane, controller saturation Model the plane of yaw state-transition matrix, design PN Guidance Law motion models in the Kalman filter side of pitch plane Journey, PN Guidance Laws motion model are in the Kalman filter equation of the plane of yaw, controller saturation motion model in pitch plane Kalman filter equation and controller saturation motion model in the Kalman filter equation of the plane of yaw, Kalman filter Device equation is Kalman filter equation；Detailed process is：

PN Guidance Laws recognize wave filter and saturation identification wave filter to use EKF, EKF be extended Kalman filter, root The state-transition matrix being calculated according to step 2,

PN Guidance Laws motion model is in the Kalman filter equation of pitch plane

PN Guidance Laws motion model is in the Kalman filter equation of the plane of yaw

Controller saturation motion model is in the Kalman filter equation of pitch plane

Controller saturation motion model is in the Kalman filter equation of the plane of yaw

In formula,It is the estimate to k+1 moment states；It is the predicted value to k+1 moment states；WithIt is the filter gain of the PN Guidance Laws motion model in pitch plane at k moment and k+1 moment respectively；WithRespectively It is the filter gain of the PN Guidance Laws motion model in the plane of yaw at k moment and k+1 moment；

WithIt is that the controller saturation motion model at k moment and k+1 moment increases in the wave filter of pitch plane respectively Benefit；

WithIt is that the controller saturation motion model at k moment and k+1 moment increases in the wave filter of the plane of yaw respectively Benefit；

For k+1 moment PN Guidance Law motion model the state forecast of pitch plane covariance matrix；

For k+1 moment PN Guidance Law motion model the state forecast of the plane of yaw covariance matrix；

For k+1 moment controller saturation motion models the state forecast of pitch plane covariance matrix；

For k+1 moment controller saturation motion models the state forecast of the plane of yaw covariance matrix；

R⁽¹⁾For PN Guidance Laws motion model pitch plane measurement noise covariance matrix；

R⁽²⁾For PN Guidance Laws motion model the plane of yaw measurement noise covariance matrix；

R⁽³⁾Measurement noise covariance matrix of the device saturation motion model in pitch plane in order to control；

R⁽⁴⁾Measurement noise covariance matrix of the device saturation motion model in the plane of yaw in order to control；

For PN Guidance Laws motion model k moment to the k+1 moment of pitch plane state-transition matrix；

For PN Guidance Laws motion model k moment to the k+1 moment of the plane of yaw state-transition matrix；

State-transition matrix of the device saturation motion model at k moment to the k+1 moment of pitch plane in order to control；

State-transition matrix of the controller saturation motion model at k moment to the k+1 moment of the plane of yaw；

Q⁽¹⁾For PN Guidance Laws motion model pitch plane process noise covariance matrix；

Q⁽²⁾For PN Guidance Laws motion model the plane of yaw process noise covariance matrix；

Q⁽³⁾Process noise covariance matrix of the device saturation motion model in pitch plane in order to control；

Q⁽⁴⁾Process noise covariance matrix of the device saturation motion model in the plane of yaw in order to control；

For PN Guidance Laws motion model pitch plane k moment state estimation covariance matrixes；

For PN Guidance Laws motion model the plane of yaw k moment state estimation covariance matrixes；

K moment state estimation covariance matrix of the device saturation motion model in pitch plane in order to control；

K moment state estimation covariance matrix of the device saturation motion model in the plane of yaw in order to control；

For PN Guidance Laws motion model pitch plane k+1 moment state estimation covariance matrixes；

For PN Guidance Laws motion model the plane of yaw k+1 moment state estimation covariance matrixes；

K+1 moment state estimation covariance matrix of the device saturation motion model in pitch plane in order to control；

K+1 moment state estimation covariance matrix of the device saturation motion model in the plane of yaw in order to control；

Turn for k moment of the PN Guidance Laws motion model in pitch plane to the state-transition matrix at k+1 moment Put；

Turn for k moment of the PN Guidance Laws motion model in the plane of yaw to the state-transition matrix at k+1 moment Put；

In order to control device saturation motion model at the k moment of pitch plane to the state-transition matrix at k+1 moment Transposition；

In order to control device saturation motion model at the k moment of the plane of yaw to the state-transition matrix at k+1 moment Transposition；

Z_k+1For the measured value at k+1 moment；I is 7 × 7 unit matrixs；H is calculation matrix, H=[1 10000 0], H^TFor the transposition of H, k is sampling instant.

Other steps and parameter are identical with one of embodiment one to three.

Embodiment five：Unlike one of present embodiment and embodiment one to four：The step 4 The middle PN Guidance Laws motion model according to step 3 exists in Kalman filter equation, the PN Guidance Law motion models of pitch plane The Kalman filter equation of the plane of yaw, controller saturation motion model are in the Kalman filter equation of pitch plane and control In the Kalman filter equation of the plane of yaw, (2 one group of pitching are a MMAE to device saturation motion model processed, and 2 yaw one Group is a MMAE), the posterior probability and saturated controller for calculating pursuer current time PN Guidance Law motion models move mould The posterior probability of type；Detailed process is：

If model set is M={ M_j| j=1 ..., r }, in formula, r is the number of model, r=2, M₁Represent PN Guidance Laws fortune Movable model, M₂Represent controller saturation motion model, be P { M in the prior probability of k moment models j_j|Z^k-1, in k moment models j Posterior probability be P { M_j|Z^k, the measured value at kth moment is z (k), and the measured value sequence definition at preceding k moment is

According to Bayes' theorem,

In formula, P { M_i|Z^k-1Be model i prior probability, p (z (k) | Z^k-1,M_i) it is likelihood letters of the model i at the k moment Number, P { M_j|z(k),Z^k-1It is k moment model Ms_jPosterior probability, p (z (k) | Z^k-1,M_j) it is likelihood letters of the model j at the k moment Number；Therefore the posterior probability of model j can be calculated by the prior probability and likelihood function of all models, the likelihood of model i Function is the distribution function that the model respective filter newly ceases；It is assumed that new breath obeys the normal distribution that average is zero, if its k moment Variance be S_i(k), i.e.,

p(z(k)|Z^k-1,M_i)=N (0, S_j(k)) (39)

In formula, N (0, S_j(k)) average is represented as 0, variance S_j(k) probability density function of normal distribution；S_j(k) it is mould The variance that type j newly ceases, i.e.,

S_j(k)=E [v (k) v^T(k)] (41)

In formula, v (k) ceases to be new, v^T(k) be v (k) transposition, E [] is mathematic expectaion function；

If the state estimation of the corresponding Kalman filters of model i isThe then state at MMAE wave filters kth moment EstimateFor

I values are 1 or 2.

Other steps and parameter are identical with one of embodiment one to four.

Embodiment six：Unlike one of present embodiment and embodiment one to five：The step 5 After the posterior probability and saturation motion model of the middle pursuer current time PN Guidance Law motion models obtained according to step 4 Test probability, calculate pursuer from controller saturation when saturation motion model be switched to controller unsaturation when PN Guidance Laws At the time of motion model；Saturation motion model when pursuer is from controller saturation is switched to PN during controller unsaturation Using controller saturation motion model in the Kalman filter equation of pitch plane and control before at the time of Guidance Law motion model Estimated result of the device saturation motion model processed in the Kalman filter equation of the plane of yaw；Otherwise using PN Guidance Laws movement mould Type pitch plane Kalman filter equation and PN Guidance Laws motion model the plane of yaw Kalman filter equation Estimated result；Detailed process is：

In view of the first stage in guidance, the external behaviors of pursuer are no longer PN Guidance Laws due to the influence of saturation Motion model, releases the motion model that saturation enters PN Guidance Laws afterwards.We are using two MMAE wave filters respectively to two The state of stage pursuer is estimated；The two MMAE wave filters are referred to as MMAE_A and MMAE_B wave filters, wherein MMAE_A wave filters estimate the first stage, i.e. pursuer is in state during controller saturation；And MMAE_B wave filters estimation the Two-stage, i.e. pursuer are in state during controller unsaturation；Additionally need estimate pursuer from controller saturation when Saturation motion model be switched to controller unsaturation when PN Guidance Law motion models at the time of, so that it is determined that in certain moment t The state of current pursuer is represented using the estimate of which MMAE wave filter；

If pursuer is introduced into state during controller saturation, the motion model of pursuer whole process is consistent, MMAE_A is consistent with the result that MMAE_B is estimated；

Pursuer from controller saturation when saturation motion model be switched to controller unsaturation when PN Guidance Laws movement Method of estimation detailed process at the time of model is：

Contain two models in each MMAE wave filters：PN Guidance Laws motion model and control when controller is unsaturated Saturation motion model during device saturation；The posterior probability of two models of present filter is calculated according to multiple model filtering method, Then according to the posterior probability of two models of present filter, satisfying when whether pursuer is in controller saturation at present is determined And the stage；MMAE_B wave filters check saturation exit point by the way of scanning, i.e., at interval of check_period long when Between, the motion model of current pursuer is judged according to formula (48), it is assumed that PN guidances when controller is unsaturated in MMAE wave filters The posterior probability of saturation motion model when rule motion model and controller saturation is respectively P { M_PNGAnd P { M_Sat, then judged Cheng Wei：

In formula, ε is threshold value.As P { M_PNG}-P{M_SatDuring } >=ε, then the model that current pursuer is used is PN Guidance Law Model, judges that pursuer has exited saturation state；Otherwise restart and initialize MMAE_B wave filters.Every check_ The period times are once judged, until judging that pursuer has exited saturation state position.Check_period is all to examine Phase, general check_period are arranged to 0.5 second.

When ε takes larger value, to the judging nicety rate higher of pursuer "current" models；Otherwise when ε takes smaller value, It is lower to the judging nicety rate of pursuer "current" models.But this does not imply that the value of ε is the bigger the better, behind can be directed to this Analyzed.

Check_period and parameter ε in relation to when ε takes very high values, it is necessary to check_period it is also larger, this will Cause the time point estimation that pursuer exits saturation inaccurate；When ε takes small value, check_period can take smaller Value.This is seen on surface can improve the precision that pursuer exits saturation time point estimation, but when ε takes small value pair The accuracy of judgement degree of pursuer "current" models can reduce.Therefore need to select suitable ε and check_period values just to take into account Pursuer "current" model accuracy of judgement degree and the accuracy for exiting the estimation of saturation moment.

Here is the pseudocode that MMAE_B exits pursuer saturation moment estimation function, and algorithm calculates pursuer and moves back Go out the backed off after random of the estimate quit_saturation_time at saturation moment.

Other steps and parameter are identical with one of embodiment one to five.

Beneficial effects of the present invention are verified using following embodiments：

Embodiment one：

A kind of proportional navigation law identification filtering method of the present embodiment is specifically to be prepared according to following steps：

The multi-model PN Guidance Laws identification wave filter that this patent proposes has carried out emulation in a missile breakthrough scene and has tested Card.In order to verify the performance of Guidance Law identification wave filter, by its estimation performance with PN Guidance Laws Kalman identification wave filters Contrasted.The wave filter does not consider controller saturated conditions, it is believed that pursuer whole process is moved under the constraint of PN Guidance Laws.

In the missile breakthrough scene, pursuer intercepts evader.It is common Guidance Law in view of PN Guidance Laws, it is false Determine evader and know that pursuer uses PN Guidance Laws in advance, but do not know its navigation constant in pitching and jaw channel. Evader can obtain the position of pursuer, use the PN Guidance Laws of multi-model PN Guidance Laws identification wave filter estimation pursuer Navigation constant and the acceleration on pitch plane and the plane of yaw.The defence that these information can pass to protection evader is led Bullet, defending missile intercept pursuer using these information, to protect evader.The simulation configurations of pursuer are in table 1 Described in, the simulation configurations of evader are described in table 2.

Table 1：Pursuer simulation configurations information

Table 2：Evader simulation configurations information

PN Guidance Laws Kalman recognizes the process noise matrix of wave filter and saturation Kalman identification wave filters and measurement is made an uproar Sound arranged in matrix is

Q_PNG=Q_Sat=diag (10,10,10,20,20,20,2)

R_PNG=R_Sat=diag (10,10,10)

Fig. 2-15 is the simulation result under the simulating scenes.Fig. 2 and Fig. 3 be evader constant value it is motor-driven in the case of, filtering Estimates of the device MMAE_A to the navigation constant of pursuer Guidance Laws.MMAE_A and MMAE_B to the estimate of navigation constant all After pursuer exits saturation true value has been converged in very short time.Pursuer is moved back in pitch channel and jaw channel controller The time for going out saturation is 2.7s and 4.6s respectively.It can be seen that being in saturation state in pursuer controllers from Fig. 2 and Fig. 3 When, PN Guidance Laws Kalman recognizes wave filter since big model error causes not converging to very the estimate of navigation constant Value.True value has been converged to the estimate of navigation constant quickly after pursuer exits saturation, has been pitch channel navigation respectively Constant is 4, and jaw channel navigation constant is 5.

Above-mentioned simulation process considers the possibility that the pursuer controllers during guidance produce saturation in the early stage, makes Pursuer Guidance Laws are recognized with multiple model filtering method, are put down to the navigation constant under PN Guidance Laws and in pitching Face and plane of yaw acceleration are estimated.More accurately Guidance Law can be recognized.In an embodiment of the present invention, if The mean square error that MMAE wave filters produce pursuer acceleration estimations is Var_MMAE；Whole process is using PN Guidance Laws to pursuer The mean square error that acceleration estimation produces is Var_PN.If α=Var_MMAEVar_PN.As 0 ＜ α ＜ 1, MMAE wave filters are to pursuer Acceleration estimation is whole compared with explanation more accurate to pursuer acceleration estimations using PN Guidance Laws；α is smaller, illustrates relative to complete For journey using PN Guidance Laws for pursuer acceleration estimations, MMAE is more accurate to pursuer acceleration estimations.

When evader without it is motor-driven when, in pursuer saturation stages, to pursuer acceleration a_yEstimation when α=0.59； To pursuer acceleration a_zEstimation when α=0.56.

Fig. 4, Fig. 5, Fig. 6, Fig. 7 be evader it is inorganic it is dynamic in the case of, whole process of guiding all uses PN Guidance Laws The estimation that Kalman identification wave filters overload pursuer in pitch channel and jaw channel when being recognized to pursuer. When pursuer controllers are in saturation state, PN Guidance Laws Kalman recognizes overload of the wave filter in pitching and jaw channel Estimate has larger difference with true value.And once pursuer controllers exit saturation, PN Guidance Laws Kalman identification filtering The estimate of device has converged to true value soon.Multiple-model estimator method proposed in this paper considers what saturation recognized acceleration Influence.The estimate for having considered MMAE_A and whole process PN Guidance Laws identification Kalman filter is as shown in Figure 4.According to calculating The pursuer gone out exits the time of saturation, and the estimation of MMAE_A wave filters is used before the controller of pursuer exits saturation Value, and the controller in pursuer is exited after saturation the estimated result of Kalman filter is recognized using whole process PN Guidance Laws.

Comparison diagram 4, Fig. 5, Fig. 6, Fig. 7 see saturation stage wave filter proposed in this paper to pursuer pitch channels and The overload estimated accuracy of jaw channel is above the estimated accuracy of PN Guidance Laws Kalman identification wave filters.And controlled in pursuer After device processed exits saturation, the estimated accuracy of two kinds of wave filters is identical.When evader execution constant value is motor-driven or sinusoidal motor-driven, knot Fruit is consistent with the result that evader is not motor-driven.As shown in Fig. 8, Fig. 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14, Figure 15.

The present invention can also have other various embodiments, in the case of without departing substantially from spirit of the invention and its essence, this area Technical staff makes various corresponding changes and deformation in accordance with the present invention, but these corresponding changes and deformation should all belong to The protection domain of appended claims of the invention.

Claims

1. a kind of proportional navigation law recognizes filtering method, it is characterised in that a kind of proportional navigation law recognizes filtering method Specifically follow the steps below：

Step 1：Pursuer has two kinds of motion models：When saturation motion model and controller during controller saturation are unsaturated PN Guidance Law motion models；State equation, the PN Guidance Law motion models that PN Guidance Laws motion model is established in pitch plane exist The state equation of the plane of yaw, controller saturation motion model pitch plane state equation and controller saturation motion model In the state equation of the plane of yaw；Evader is ballistic missile；Pursuer is interception guided missile；PN Guidance Laws are proportional guidance system Lead rule；

Step 2：According to state equation of the PN Guidance Laws motion model of step 1 in pitch plane, PN Guidance Law motion models State equation, controller saturation motion model in the plane of yaw move mould in the state equation and controller saturation of pitch plane Type is in the state equation of the plane of yaw, state-transition matrix, PN Guidance Law of the calculating PN Guidance Laws motion model in pitch plane Motion model the state-transition matrix of the plane of yaw, controller saturation motion model pitch plane state-transition matrix, State-transition matrix of the controller saturation motion model in the plane of yaw；

Step 3：Moved according to state-transition matrix, PN Guidance Law of the PN Guidance Laws motion model of step 2 in pitch plane Model is in the state-transition matrix of the state-transition matrix of the plane of yaw, controller saturation motion model in pitch plane, control Device saturation motion model is in the state-transition matrix of the plane of yaw, Kalman of the design PN Guidance Laws motion model in pitch plane Filter equation, PN Guidance Laws motion model exist in Kalman filter equation, the controller saturation motion model of the plane of yaw The Kalman filter equation and controller saturation motion model of pitch plane the plane of yaw Kalman filter equation, Kalman filter equation is Kalman filter equation；

Step 4：According to Kalman filter equation of the PN Guidance Laws motion model of step 3 in pitch plane, PN Guidance Laws Motion model the Kalman filter equation of the plane of yaw, controller saturation motion model pitch plane Kalman filter Device equation and controller saturation motion model are at the Kalman filter equation of the plane of yaw, calculating pursuer current times PN The posterior probability of Guidance Law motion model and the posterior probability of controller saturation motion model；

Step 5：Posterior probability and the saturation fortune of the pursuer current time PN Guidance Law motion models obtained according to step 4 The posterior probability of movable model, calculate pursuer from controller saturation when saturation motion model be switched to controller unsaturation when PN Guidance Law motion models at the time of；Saturation motion model when pursuer is from controller saturation is switched to controller not Before at the time of PN Guidance Law motion models during saturation using controller saturation motion model pitch plane Kalman filter The estimated result of device equation and controller saturation motion model in the Kalman filter equation of the plane of yaw；Otherwise PN systems are used Rule motion model is led in the Kalman filter equation and PN Guidance Law motion models of pitch plane in the plane of yaw

The estimated result of Kalman filter equation.

A kind of 2. proportional navigation law identification filtering method according to claim 1, it is characterised in that：In the step 1 Pursuer has two kinds of motion models：PN Guidance Laws fortune when saturation motion model and controller during controller saturation are unsaturated Movable model；PN Guidance Laws motion model is established in the state equation of pitch plane, PN Guidance Law motion models in the plane of yaw State equation, controller saturation motion model pitch plane state equation and controller saturation motion model in the plane of yaw State equation, pursuer is interception guided missile；Detailed process is：

In missile breakthrough scene, anti-ballistic missile of dashing forward will escape blocking for the interception guided missile of other side's missile defense systems transmitting Cut, be escape side, be known as evader, interception guided missile is the side of chasing, and is known as pursuer, and pursuer there are two kinds of motion models：Control PN Guidance Law motion models when saturation motion model and controller during device saturation processed are unsaturated；

The PN Guidance Laws equation of motion modeling of pursuer, r_xIt is evader and pursuer relative positions in evader inertial system x-axis Under component；r_yFor component of the evader and pursuer relative positions under evader inertial system y-axis；r_zFor evader and Component of the pursuer relative positions under evader inertial system z-axis；Evader is ballistic missile, and pursuer is interception guided missile；

r_x=x_p-x_e r_y=y_p-y_e r_z=z_p-z_e (1)

In formula, [x_p,y_p,z_p]^T[x_e,y_e,z_e]^TIt is the position of pursuer and evader under evader inertial systems respectively, T is Transposition, x_pFor positions of the pursuer under evader inertial system x-axis, y_pFor positions of the pursuer under evader inertial system y-axis Put, z_pFor positions of the pursuer under evader inertial system z-axis, x_eFor positions of the evader under evader inertial system x-axis, y_e For positions of the evader under evader inertial system y-axis, z_eFor positions of the evader under evader inertial system z-axis；

PN Guidance Laws motion model is in the state equation of pitch plane

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&CenterDot;</mo> </mover> <mi>y</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>y</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>x</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&CenterDot;</mo> </mover> <mi>y</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>y</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>y</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <mfrac> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mi>&tau;</mi> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>y</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>N</mi> <mi>&epsiv;</mi> </msub> <mfrac> <mrow> <mo>(</mo> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>x</mi> </msub> <mo>+</mo> <msub> <mi>r</mi> <mi>y</mi> </msub> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>)</mo> <mo>(</mo> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>-</mo> <msub> <mi>r</mi> <mi>y</mi> </msub> <msub> <mi>v</mi> <mi>x</mi> </msub> <mo>)</mo> </mrow> <mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <mfrac> <mn>3</mn> <mn>2</mn> </mfrac> </msup> <mi>&tau;</mi> </mrow> </mfrac> <mo>-</mo> <mfrac> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>y</mi> </mrow> </msub> <mi>&tau;</mi> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>N</mi> <mo>&CenterDot;</mo> </mover> <mi>&epsiv;</mi> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>

PN Guidance Laws motion model is in the state equation of the plane of yaw

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&CenterDot;</mo> </mover> <mi>z</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>x</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&CenterDot;</mo> </mover> <mi>z</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>z</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>z</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <mfrac> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mi>&tau;</mi> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>z</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <mfrac> <msub> <mi>N</mi> <mi>&beta;</mi> </msub> <mi>&tau;</mi> </mfrac> <mfrac> <mrow> <mo>(</mo> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>x</mi> </msub> <mo>+</mo> <msub> <mi>r</mi> <mi>z</mi> </msub> <msub> <mi>v</mi> <mi>z</mi> </msub> <mo>)</mo> <mo>(</mo> <msub> <mi>r</mi> <mi>z</mi> </msub> <msub> <mi>v</mi> <mi>x</mi> </msub> <mo>-</mo> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>z</mi> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>z</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <mfrac> <mn>3</mn> <mn>2</mn> </mfrac> </msup> </mfrac> <mo>-</mo> <mfrac> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>z</mi> </mrow> </msub> <mi>&tau;</mi> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>N</mi> <mo>&CenterDot;</mo> </mover> <mi>&beta;</mi> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>

In formula, v_xFor component of the evader and pursuer relative velocities under evader inertial system x-axis, v_yFor evader and Component of the pursuer relative velocities under evader inertial system y-axis, v_zIt is evader and pursuer relative velocities in evader Component under inertial system z-axis；a_pxFor component of the acceleration under evader inertial system x-axis of pursuer, a_pyFor pursuer's Component of the acceleration under evader inertial system y-axis, a_pzFor component of the acceleration under evader inertial system z-axis of pursuer； a_ex, a_eyAnd a_ezIt is component of the acceleration of evader under evader inertial systems, N_εIt is navigation of the pursuer in pitch plane Constant；N_βIt is navigation constants of the pursuer in the plane of yaw；τ is time constant,For r_xFirst derivative,For r_ySingle order Derivative,For r_zFirst derivative,For v_xFirst derivative,For v_yFirst derivative,For v_zFirst derivative,For a_px First derivative,For a_pyFirst derivative,For a_pzFirst derivative,For N_εFirst derivative,For N_βSingle order Derivative；

Assuming that evader obtains positions of the pursuer under inertial system, and then calculates the opposite position of itself, calculate opposite The measurement equation of the position of itself is

H=[r_x,r_y,r_z]^T (4)

Controller saturation motion model is in the state equation of pitch plane

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&CenterDot;</mo> </mover> <mi>y</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>y</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>x</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&CenterDot;</mo> </mover> <mi>y</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>y</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>y</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <mfrac> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mi>&tau;</mi> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>y</mi> </mrow> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>d</mi> <mo>&CenterDot;</mo> </mover> <mn>1</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>

Controller saturation motion model is in the state equation of the plane of yaw

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&CenterDot;</mo> </mover> <mi>z</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>x</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&CenterDot;</mo> </mover> <mi>z</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>z</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>z</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <mfrac> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mi>&tau;</mi> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>z</mi> </mrow> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>d</mi> <mo>&CenterDot;</mo> </mover> <mn>2</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>

In formula, d₁And d₂It is the occupied state of controller saturation motion model.

A kind of 3. proportional navigation law identification filtering method according to claim 2, it is characterised in that：In the step 2 According to the PN Guidance Laws motion model of step 1 in the state equation of pitch plane, PN Guidance Law motion models in the plane of yaw State equation, controller saturation motion model pitch plane state equation and controller saturation motion model in the plane of yaw State equation, calculate PN Guidance Laws motion model in the state-transition matrix of pitch plane, PN Guidance Law motion models inclined The navigate state-transition matrix of plane, controller saturation motion model is transported in the state-transition matrix of pitch plane, controller saturation State-transition matrix of the movable model in the plane of yaw；Detailed process is：

PN Guidance Laws motion model is in the state-transition matrix of pitch plane

<mrow> <msub> <mi>a</mi> <mn>1</mn> </msub> <mo>=</mo> <mo>-</mo> <mfrac> <mrow> <msub> <mi>TN</mi> <mi>&epsiv;</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>3</mn> </msubsup> <msub> <mi>v</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>-</mo> <mn>2</mn> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <msub> <mi>r</mi> <mi>y</mi> </msub> <msubsup> <mi>v</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <mn>2</mn> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <msub> <mi>r</mi> <mi>y</mi> </msub> <msubsup> <mi>v</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>-</mo> <mn>5</mn> <msub> <mi>r</mi> <mi>x</mi> </msub> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <msub> <mi>v</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>+</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>3</mn> </msubsup> <msubsup> <mi>v</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>3</mn> </msubsup> <msubsup> <mi>v</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <mrow> <mn>5</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> <mi>&tau;</mi> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <msub> <mi>a</mi> <mn>2</mn> </msub> <mo>=</mo> <mo>-</mo> <mfrac> <mrow> <msub> <mi>TN</mi> <mi>&epsiv;</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>3</mn> </msubsup> <msubsup> <mi>v</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>3</mn> </msubsup> <msubsup> <mi>v</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>+</mo> <mn>5</mn> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <msub> <mi>r</mi> <mi>y</mi> </msub> <msub> <mi>v</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>-</mo> <mn>2</mn> <msub> <mi>r</mi> <mi>x</mi> </msub> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <msubsup> <mi>v</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <mn>2</mn> <msub> <mi>r</mi> <mi>x</mi> </msub> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <msubsup> <mi>v</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>3</mn> </msubsup> <msub> <mi>v</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <mrow> <mn>5</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> <mi>&tau;</mi> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <msub> <mi>a</mi> <mn>3</mn> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>TN</mi> <mi>&epsiv;</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>-</mo> <mn>2</mn> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>r</mi> <mi>y</mi> </msub> <msub> <mi>v</mi> <mi>x</mi> </msub> <mo>-</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <mrow> <mn>3</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> <mi>&tau;</mi> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <msub> <mi>a</mi> <mn>4</mn> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>TN</mi> <mi>&epsiv;</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <msub> <mi>v</mi> <mi>x</mi> </msub> <mo>+</mo> <mn>2</mn> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>r</mi> <mi>y</mi> </msub> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>-</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <msub> <mi>v</mi> <mi>x</mi> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <mrow> <mn>3</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> <mi>&tau;</mi> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>

In formula, T is measurement period, and unit is the second；

A kind of 4. proportional navigation law identification filtering method according to claim 3, it is characterised in that：In the step 3 It is flat in yaw in state-transition matrix, the PN Guidance Laws motion model of pitch plane according to the PN Guidance Laws motion model of step 2 State-transition matrix, controller saturation movement mould of the state-transition matrix, controller saturation motion model in face in pitch plane Type the plane of yaw state-transition matrix, design PN Guidance Laws motion model pitch plane Kalman filter equation, PN Guidance Laws motion model is in the Kalman filter equation of the plane of yaw, controller saturation motion model in pitch plane Kalman filter equation and controller saturation motion model are in the Kalman filter equation of the plane of yaw, Kalman filter Equation is Kalman filter equation；Detailed process is：

PN Guidance Laws recognize wave filter and saturation identification wave filter uses EKF, and EKF is extended Kalman filter, according to step Rapid two state-transition matrixes being calculated,

PN Guidance Laws motion model is in the Kalman filter equation of pitch plane

In formula,It is the estimate to k+1 moment states；It is the predicted value to k+1 moment states；

WithIt is the filter gain of the PN Guidance Laws motion model in pitch plane at k moment and k+1 moment respectively；

WithIt is the filter gain of the PN Guidance Laws motion model in the plane of yaw at k moment and k+1 moment respectively；

WithIt is the filter gain of the controller saturation motion model in pitch plane at k moment and k+1 moment respectively；

WithIt is the filter gain of the controller saturation motion model in the plane of yaw at k moment and k+1 moment respectively；

R⁽¹⁾For PN Guidance Laws motion model pitch plane measurement noise matrix；

R⁽²⁾For PN Guidance Laws motion model the plane of yaw measurement noise matrix；

R⁽³⁾Measurement noise matrix of the device saturation motion model in pitch plane in order to control；

R⁽⁴⁾Measurement noise matrix of the device saturation motion model in the plane of yaw in order to control；

State-transition matrix of the device saturation motion model at k moment to the k+1 moment of the plane of yaw in order to control；

Q⁽¹⁾For PN Guidance Laws motion model pitch plane process noise matrix；

Q⁽²⁾For PN Guidance Laws motion model the plane of yaw process noise matrix；

Q⁽³⁾Process noise matrix of the device saturation motion model in pitch plane in order to control；

Q⁽⁴⁾Process noise matrix of the device saturation motion model in the plane of yaw in order to control；

For PN Guidance Laws motion model pitch plane the k moment to the state-transition matrix at k+1 moment transposition；

For PN Guidance Laws motion model the plane of yaw the k moment to the state-transition matrix at k+1 moment transposition；

Device saturation motion model turns at the k moment of pitch plane to the state-transition matrix at k+1 moment in order to control Put；

Device saturation motion model turns at the k moment of the plane of yaw to the state-transition matrix at k+1 moment in order to control Put；

Z_k+1For the measured value at k+1 moment；I is 7 × 7 unit matrixs；H is calculation matrix, H=[1 10000 0], H^TFor H Transposition, k is sampling instant.

A kind of 5. proportional navigation law identification filtering method according to claim 4, it is characterised in that：In the step 4 According to the PN Guidance Laws motion model of step 3 in the Kalman filter equation of pitch plane, PN Guidance Law motion models inclined The Kalman filter equation of plane, controller saturation motion model navigate in the Kalman filter equation of pitch plane and control Device saturation motion model is in the Kalman filter equation of the plane of yaw, calculating pursuer current times PN Guidance Law movement mould The posterior probability of type and the posterior probability of saturated controller motion model；Detailed process is：

If model set is M={ M_j| j=1 ..., r }, in formula, r is the number of model, r=2, M₁Represent PN Guidance Laws movement mould Type, M₂Represent controller saturation motion model, be P { M in the prior probability of k moment models j_j|Z^k-1, after k moment models j It is P { M to test probability_j|Z^k, the measured value at kth moment is z (k), and the measured value sequence definition at preceding k moment is

<mrow> <msup> <mi>Z</mi> <mi>k</mi> </msup> <mover> <mo>=</mo> <mi>&Delta;</mi> </mover> <mo>{</mo> <mi>z</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>|</mo> <mi>k</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mi>k</mi> <mo>}</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>37</mn> <mo>)</mo> </mrow> </mrow>

According to Bayes' theorem,

<mrow> <mi>P</mi> <mo>{</mo> <msub> <mi>M</mi> <mi>j</mi> </msub> <mo>|</mo> <msup> <mi>Z</mi> <mi>k</mi> </msup> <mo>}</mo> <mo>=</mo> <mi>P</mi> <mo>{</mo> <msub> <mi>M</mi> <mi>j</mi> </msub> <mo>|</mo> <mi>z</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>,</mo> <msup> <mi>Z</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>}</mo> <mo>=</mo> <mfrac> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <mi>z</mi> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mo>|</mo> <msup> <mi>Z</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>,</mo> <msub> <mi>M</mi> <mi>j</mi> </msub> <mo>)</mo> </mrow> <mi>P</mi> <mo>{</mo> <msub> <mi>M</mi> <mi>j</mi> </msub> <mo>|</mo> <msup> <mi>Z</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>}</mo> </mrow> <mrow> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>r</mi> </munderover> <mi>p</mi> <mrow> <mo>(</mo> <mi>z</mi> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mo>|</mo> <msup> <mi>Z</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>,</mo> <msub> <mi>M</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mi>P</mi> <mo>{</mo> <msub> <mi>M</mi> <mi>i</mi> </msub> <mo>|</mo> <msup> <mi>Z</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>}</mo> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>38</mn> <mo>)</mo> </mrow> </mrow>

In formula, P { M_i|Z^k-1Be model i prior probability, p (z (k) | Z^k-1,M_i) for model i in the likelihood function at k moment, P {M_j|z(k),Z^k-1It is k moment model Ms_jPosterior probability, p (z (k) | Z^k-1,M_j) it is likelihood functions of the model j at the k moment；Cause The posterior probability of this model j can be calculated by the prior probability and likelihood function of all models, and the likelihood function of model i is The distribution function that the model respective filter newly ceases；It is assumed that new breath obeys the normal distribution that average is zero, if the variance at its k moment For S_i(k), i.e.,

p(z(k)|Z^k-1,M_i)=N (0, S_j(k)) (39)

In formula, N (0, S_j(k)) average is represented as 0, variance S_j(k) probability density function of normal distribution；S_j(k) it is model j The variance newly ceased, i.e.,

S_j(k)=E [v (k) v^T(k)] (41)

In formula, v (k) ceases to be new, v^T(k) be v (k) transposition, E [] is mathematic expectaion function.

A kind of 6. proportional navigation law identification filtering method according to claim 5, it is characterised in that：In the step 5 The posterior probability of pursuer current time PN Guidance Law motion models and the posteriority of saturation motion model obtained according to step 4 Probability, calculate pursuer from controller saturation when saturation motion model be switched to controller unsaturation when PN Guidance Laws fortune At the time of movable model；Saturation motion model when pursuer is from controller saturation is switched to PN systems during controller unsaturation Controller saturation motion model is used before at the time of leading rule motion model in the Kalman filter equation of pitch plane and control Estimated result of the device saturation motion model in the Kalman filter equation of the plane of yaw；Otherwise PN Guidance Law motion models are used Pitch plane Kalman filter equation and PN Guidance Laws motion model the plane of yaw Kalman filter equation Estimated result.Detailed process is：

The state of two stage pursuer is estimated respectively using two MMAE wave filters；By the two MMAE wave filters MMAE_A and MMAE_B wave filters are referred to as, wherein MMAE_A wave filters estimation first stage, i.e. pursuer is in controller State during saturation；And MMAE_B wave filters estimation second stage, i.e. pursuer are in state during controller unsaturation；Separately It is outer need to estimate pursuer from controller saturation when saturation motion model be switched to controller unsaturation when PN Guidance Laws At the time of motion model, so that it is determined that representing current pursuer using the estimate of which MMAE wave filter in certain moment t State；

If pursuer is introduced into state during controller saturation, the motion model of pursuer whole process is consistent, MMAE_A It is consistent with the result that MMAE_B is estimated；

Pursuer from controller saturation when saturation motion model be switched to controller unsaturation when PN Guidance Law motion models At the time of method of estimation detailed process be：

Contain two models in each MMAE wave filters：PN Guidance Laws motion model and controller when controller is unsaturated are satisfied With when saturation motion model；The posterior probability of two models of present filter is calculated according to multiple model filtering method, then According to the posterior probability of two models of present filter, saturation rank when whether pursuer is in controller saturation at present is determined Section；MMAE_B wave filters check saturation exit point, the i.e. time at interval of check_period long by the way of scanning, Check_period is round of visits, the motion model of current pursuer is judged according to formula (48), it is assumed that controlled in MMAE wave filters The posterior probability of saturation motion model during PN Guidance Laws motion model and controller saturation when device processed is unsaturated is respectively P {M_PNGAnd P { M_Sat, then deterministic process is：

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>mod</mi> <mi>e</mi> <mo>=</mo> <mi>P</mi> <mi>N</mi> <mi>G</mi> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mi>f</mi> <mi> </mi> <mi>P</mi> <mo>{</mo> <msub> <mi>M</mi> <mrow> <mi>P</mi> <mi>N</mi> <mi>G</mi> </mrow> </msub> <mo>}</mo> <mo>-</mo> <mi>P</mi> <mo>{</mo> <msub> <mi>M</mi> <mrow> <mi>S</mi> <mi>a</mi> <mi>t</mi> </mrow> </msub> <mo>}</mo> <mo>&GreaterEqual;</mo> <mi>&epsiv;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>mod</mi> <mi>e</mi> <mo>=</mo> <mi>S</mi> <mi>a</mi> <mi>t</mi> <mi>u</mi> <mi>r</mi> <mi>a</mi> <mi>t</mi> <mi>i</mi> <mi>o</mi> <mi>n</mi> </mrow> </mtd> <mtd> <mrow> <mi>e</mi> <mi>l</mi> <mi>s</mi> <mi>e</mi> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>48</mn> <mo>)</mo> </mrow> </mrow>

In formula, ε is threshold value；As P { M_PNG}-P{M_SatDuring } >=ε, then the model that current pursuer is used is PN Guidance Law mould Type, judges that pursuer has exited saturation state；Otherwise restart and initialize MMAE_B wave filters, check_period is set It is set to 0.5 second.