CN105486307A - Line-of-sight angular rate estimating method of maneuvering target - Google Patents

Abstract

The invention relates to a line-of-sight angular rate estimating method, and concretely relates to a line-of-sight angular rate estimating method of a maneuvering target. The purpose of the invention is to solve the problem of limited precision of the line-of-sight angular rate extraction of the maneuvering target of present maneuvering target line-of-sight angular rate estimating methods. The method provided by the invention comprises the following steps: measuring and calculating a relative distance R from the target to a missile and a relative speed R<.> between the target and the missile, measuring and calculating, the acceleration components a epsilon and a beta of the missile, estimating the components a tepsilon and a tbeta of the target, and calculating the line-of-sight pitch angle q epsilon and the line-of-sight yaw angle q beta; and respectively substituting the a epsilon, the a beta, the a tepsilon, the a tbeta, the q epsilon and the q beta to a pitching channel line-of-sight angular rate Kalman filter and a yam channel line-of-sight angular rate Kalman filter of the missile in order to accurately obtain the line-of-sight pitch angular rate and the line-of-sight yam angular rate between the target and the missile. The method is suitable for estimating the line-of-sight angular rate of maneuvering targets in the missile making field.

Description

Line-of-sight angular rate estimation method for maneuvering target

Technical Field

The invention relates to a line-of-sight angular rate estimation method.

Background

In the technical field of guidance, the continuous maneuvering of the target greatly increases the difficulty of high-precision extraction of the line-of-sight angular rate, the existing line-of-sight angular rate extraction method cannot meet the requirement of precise guidance of intercepting a missile or an interceptor directly colliding with the target, and a new high-precision line-of-sight angular rate extraction method adaptive to the maneuvering condition of the target must be researched.

In recent years, in order to reduce the cost and improve the reliability, a small-sized strapdown or semi-strapdown seeker is increasingly adopted for accurately guiding weapons, and the seeker is characterized in that the relative line-of-sight angle between a target and a missile can be measured, if an active seeker is available, distance information can be obtained, but the strapdown or semi-strapdown seeker cannot directly output line-of-sight angle speed information required by an actual guidance law.

The existing line-of-sight angular rate estimation methods are all aimed at non-maneuvering targets, and the extraction precision of the line-of-sight angular rate is limited when the maneuvering targets are dealt with. Therefore, no effective line-of-sight angular rate high-precision extraction method exists at present aiming at the application background of missile interception maneuvering targets provided with strapdown or semi-strapdown guidance heads.

Disclosure of Invention

The invention aims to solve the problem that the precision of extracting the line-of-sight angular rate of a maneuvering target is limited by the existing line-of-sight angular rate estimation method for a non-maneuvering target, and further provides a high-precision Kalman filtering method for the line-of-sight angular rate of a strapdown or semi-strapdown seeker and the maneuvering target.

A method of estimating line-of-sight angular rate for a motorized target, comprising the steps of:

firstly, measuring and calculating a target-missile relative distance R and a target-missile relative speed R;

step two, measuring and calculating the acceleration of the missile in a sight line coordinate system o' x₄y₄z₄O' y of₄And o' z₄In the axial directionComponent a And a_β；

Step three, estimating the target acceleration in a sight line coordinate system o' x₄y₄z₄O' y of₄And o' z₄Component a in the axial direction_tAnd a_tβ；

Step four, calculating the view pitch angle q And line of sight yaw angle q_β；

Step five, the relative distance R between the target and the missile, the relative speed R between the target and the missile and the acceleration component a of the missile And a_βTarget acceleration estimation result a_tAnd a_tβAnd the calculation result q of the view pitch angle and the view yaw angle And q is_βAnd respectively substituting the angular rate of the pitching channel line of sight into a Kalman filter and the angular rate of the yawing channel line of sight into the Kalman filter of the guided missile, thereby accurately solving the line of sight pitch angle rate and the line of sight yaw rate between the target and the guided missile.

The invention has the following beneficial effects:

the target acceleration estimation Kalman filter and the sight line angular rate estimation Kalman filter are combined to obtain the strapdown or semi-strapdown seeker sight line angular rate estimation Kalman filter aiming at the maneuvering target, the precision of the Kalman filter is higher than that of the traditional strapdown or semi-strapdown seeker sight line angular rate estimation Kalman filter aiming at the non-maneuvering target, and the precision is improved by 0.03 degree/s.

Drawings

FIG. 1 is a schematic diagram showing a relationship among a geocentric inertial coordinate system, a launching point inertial coordinate system and a terminal guidance initial sight line coordinate system;

FIG. 2 is a schematic diagram of a relationship between an initial sight line coordinate system and a sight line coordinate system of the terminal guidance;

FIG. 3 is a schematic diagram of a missile coordinate system of a missile;

FIG. 4 is a diagram of the simulation effect of the longitudinal flight trajectory of the missile interception target;

FIG. 5 is a diagram of the simulation effect of the lateral flight trajectory of the missile interception target; a

FIG. 6 is a view of a simulation effect of the target-missile view pitch angle rate;

FIG. 7 is a diagram of a target-missile line of sight yaw rate simulation effect;

FIG. 8 emission point inertial coordinate system x_FAn axis target acceleration and an estimated simulation effect graph thereof;

FIG. 9 inertial coordinate system of emission points y_FAn axis target acceleration and an estimated simulation effect graph thereof;

FIG. 10 inertial coordinate system of emission points z_FAnd (4) an axis target acceleration and an estimated simulation effect graph thereof.

Detailed Description

The first embodiment is as follows:

a method of estimating line-of-sight angular rate for a motorized target, comprising the steps of:

firstly, measuring and calculating a target-missile relative distance R and a target-missile relative speed R;

step two, measuring and calculating the acceleration of the missile in a sight line coordinate system o' x₄y₄z₄O' y of₄And o' z₄Component a in the axial direction And a_β；

Step three, estimating the target acceleration in a sight line coordinate system o' x₄y₄z₄O' y of₄And o' z₄Component a in the axial direction_tAnd a_tβ；

Step four, calculating the view pitch angle q And line of sight yaw angle q_β；

Step five, the relative distance R between the target and the missile, the relative speed R between the target and the missile and the acceleration component a of the missile And a_βTarget acceleration estimation result a_tAnd a_tβAnd the calculation result q of the view pitch angle and the view yaw angle And q is_βAnd respectively substituting the angular rate of the pitching channel line of sight into a Kalman filter and the angular rate of the yawing channel line of sight into the Kalman filter of the guided missile, thereby accurately solving the line of sight pitch angle rate and the line of sight yaw rate between the target and the guided missile.

The second embodiment is as follows: the present embodiment is described in connection with figure 1,

the specific steps for measuring and calculating the target-missile relative distance R and the target-missile relative speed R in the first step of the embodiment are as follows:

defining a geocentric inertial coordinate system: center of earth inertial coordinate system o_Ix_Iy_Iz_IOrigin o of_ILocated on the center of the earth o_Ix_IThe axis lying in the equatorial plane and pointing to a certain star, o_Iz_IThe axis pointing in the direction of the north pole, o_Iy_IDetermining according to the right-hand rule;

defining an inertial coordinate system of the emission point: setting an inertial coordinate system o of a transmitting point_Fx_Fy_Fz_FThe missile is fixed relative to the earth center inertial coordinate system at the launching moment of the missile, the relation between the missile and the earth center inertial coordinate system is shown in figure 1, and the origin of the launching point inertial coordinate system is a launching point o_F，o_Fy_FAxial plumb upward, o_Fx_FShaft and o_Fz_FThe axes lying in a horizontal plane, the orientation of which is chosen as desired, usually o_Fx_Fy_FIs a shooting plane;

referring to FIG. 1, the transformation between the Earth's center inertial frame and the launch point inertial frame is determined byλ₀And α_FThree angles;λ₀and α_FRespectively the latitude, longitude and transmitting azimuth of the transmitting point relative to the geocentric inertial coordinate system;

defining an end guidance initial sight line coordinate system: o in FIG. 1₀x₀y₀z₀For final guidance of the initial line-of-sight coordinate system, o₀The missile seeker is positioned on the rotation center of the missile seeker at the final guidance initial moment; o₀x₀The initial sight direction is the pointing target is positive; o₀y₀Is located at and contains o₀x₀In the vertical plane of (a), perpendicular to o₀x₀Pointing upward is positive; o₀z₀Determining according to the right-hand rule; during the whole final guidance process, o₀x₀y₀z₀The coordinate system is relative to the inertial coordinate system o of the transmitting point_Fx_Fy_Fz_FCuring is still; q. q.s₀And q is_β0Respectively an initial sight line pitch angle and a sight line yaw angle formed by the sight line relative to an inertial coordinate system of the emission point;

the position of the missile in the inertial coordinate system of the launching point is (x, y, z), and the projection of the velocity of the missile in the inertial coordinate system is (V)_x、V_y、V_z) The information is provided by the on-board navigation system;

the position of the target in the inertial coordinate system of the emitting point is (x)_t、y_t、z_t) The projection of its velocity in the coordinate system is (V)_tx、V_ty、V_tz) This information is provided by the on-board target tracking filter;

according to △ x ═ x-x_t，△y＝y-y_t，△z＝z-z_t，△V_x＝V_x-V_tx，△V_y＝V_y-V_ty，△V_z＝V_z-V_tzTo calculate the eyesRelative distance between target and missileAnd target-missile relative velocity $\stackrel{\&CenterDot;}{R}=({\mathrm{\&Delta;x\&Delta;V}}_x+{\mathrm{\&Delta;y\&Delta;V}}_y+{\mathrm{\&Delta;z\&Delta;V}}_z)/R;$ In the final guidance process, there is always R>0， $\stackrel{\&CenterDot;}{R}<0.$

Other steps and parameters are the same as in the first embodiment.

The third concrete implementation mode:

in the second step of the present embodiment, the missile acceleration is measured and calculated in the sight line coordinate system o' x₄y₄z₄O' y of₄And o' z₄Component a in the axial direction And a_βThe method comprises the following specific steps:

defining a line-of-sight coordinate system: line of sight coordinate system o' x₄y₄z₄The origin o' is positioned at the rotation center of the seeker; o' x₄The axis is consistent with the sight of the target missile at the current moment, and the direction from the rotation center of the seeker to the target is positive; o' y₄The axis being located containing o' x₄Within the plumb plane of the shaft, with o' x₄The axis is vertical, and the pointing direction is positive; o' z₄The axis is determined according to the right-hand rule; as shown in fig. 2, the initial sight line coordinate system is converted into sight from terminal guidanceLine coordinate system definition view pitch angle q And line of sight yaw angle q_β；

Defining a projectile coordinate system; shown in FIG. 3, missile body coordinate system ox₁y₁z₁Is located on the missile centroid; ox₁The axis is coincident with the longitudinal axis of the projectile body, and the pointing head is positive; oy₁With axis in the longitudinal symmetry plane of the projectile body, perpendicular to ox₁A shaft pointing upward as positive; oz₁The axial direction is determined according to the right-hand rule;

acceleration output by accelerometer on missileIs along the body coordinate system ox₁y₁z₁Projected acceleration from accelerometer on missileProjected to a line of sight coordinate system o' x₄y₄z₄O' y of₄And o' z₄Obtaining missile sight line coordinate system o' x in axial direction₄y₄z₄O' y of₄And o' z₄Acceleration a in the axial direction And a_β。

Other steps and parameters are the same as in the first or second embodiment.

The fourth concrete implementation mode:

step three of the present embodiment is to estimate the target acceleration in the sight line coordinate system o' x₄y₄z₄O' y of₄And o' z₄Component a in the axial direction_tAnd a_tβThe method comprises the following specific steps:

step 3.1, respectively describing the target acceleration component a by using Singer models on three axes of the inertial coordinate system of the launching point_tx、a_tyAnd a_tz：

${\stackrel{\&CenterDot;}{a}}_{tx}=-{\mathrm{\&lambda;}}_x{a}_{tx}+w_x$

${\stackrel{\&CenterDot;}{a}}_{ty}=-{\mathrm{\&lambda;}}_y{a}_{ty}+w_y$

${\stackrel{\&CenterDot;}{a}}_{tz}=-{\mathrm{\&lambda;}}_z{a}_{tz}+w_z$

Wherein λ is_x、λ_yAnd λ_zRespectively representing the reciprocal of a target maneuvering time constant in the directions of three axes of an inertial coordinate system of the launching point; w is a_x、w_yAnd w_zRespectively representing zero mean Gaussian white noise on three axes;anddenotes a_tx、a_tyAnd a_tzThe upper band of the parameter, each representing a derivative of the parameter; target acceleration component a_tx、a_tyAnd a_tzThe acceleration generated by aerodynamic force and the acceleration generated by gravity are included;

three sets of motion equations are established

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_t=V_x\\ {\stackrel{\&CenterDot;}{V}}_{tx}=a_{tx}\\ {\stackrel{\&CenterDot;}{a}}_{tx}=-{\mathrm{\&lambda;}}_x{a}_{tx}+w_x\end{array}\right.---\left(1\right)$

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{y}}_t=V_{ty}\\ {\stackrel{\&CenterDot;}{V}}_n=a_{ty}\\ {\stackrel{\&CenterDot;}{a}}_{ty}=-{\mathrm{\&lambda;}}_y{a}_{ty}+w_y\end{array}\right.---\left(2\right)$

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{z}}_t=V_{tz}\\ {\stackrel{\&CenterDot;}{V}}_{tz}=a_{tz}\\ {\stackrel{\&CenterDot;}{a}}_{tz}=-{\mathrm{\&lambda;}}_z{a}_{tz}+w_z\end{array}\right.---\left(3\right)$

Step 3.2, with respect to equation (1), a target acceleration component a is defined_txO in the inertial frame of the emission point_Fx_FState variable X of shaft_x＝[x_tV_txa_tx]^TThen the following equation of state is obtained

${\stackrel{\&CenterDot;}{X}}_x=F_x{X}_x---\left(4\right)$

Wherein,

$F_x=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& -{\mathrm{\&lambda;}}_x\end{array}\right]---\left(5\right)$

discretizing the formula (5) by a sampling period delta t (the period is consistent with the guidance period) to obtain

${\mathrm{\&Phi;}}_x=\exp \left(F_x\mathrm{\&Delta;}t\right)=\left[\begin{array}{ccc}1& \mathrm{\&Delta;}t& 1/{\mathrm{\&lambda;}}_x^2(e^{-{\mathrm{\&lambda;}}_x\mathrm{\&Delta;}t}+{\mathrm{\&lambda;}}_x\mathrm{\&Delta;}t-1)\\ 0& 1& 1/{\mathrm{\&lambda;}}_x(1-e^{-{\mathrm{\&lambda;}}_x\mathrm{\&Delta;}t})\\ 0& 0& e^{-{\mathrm{\&lambda;}}_x\mathrm{\&Delta;}t}\end{array}\right]---\left(6\right)$

The discretized equation of state is

X_x(k+1)＝Φ_xX_x(k)(7)

Wherein k represents the kth sampling instant;

the position of the target is measured by a ground radar measuring device, and the measurement equation is

η_x(k)＝x_t(k) (8) the update rate of the measurement information data is low, for example, one update of 0.2 s;

according to the model as formula (1), the following emission point inertial coordinate system x is designed_FA Kalman filter for target tracking of axis with prediction equation of

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{X}}_x(k+1)={\mathrm{\&Phi;}}_x{\hat{X}}_x\left(k\right)\\ P_x(k+1/k)={\mathrm{\&Phi;}}_x{P}_x\left(k\right){\mathrm{\&Phi;}}_x^T+Q_x\end{array}\right.---\left(9\right)$

Wherein,andeach represents X_xFiltering estimation and prediction estimation of (1); q_xPredicting an error covariance matrix for the model;

$P_x(k+1/k)=E\{\&lsqb;X_x(k+1)-{\stackrel{\&OverBar;}{X}}_x(k+1)\&rsqb;{\&lsqb;X_x(k+1)-{\stackrel{\&OverBar;}{X}}_x(k+1)\&rsqb;}^T\},$

$P_x(k+1)=E\{\&lsqb;X_x(k+1)-{\hat{X}}_x(k+1)\&rsqb;{\&lsqb;X_x(k+1)-{\hat{X}}_x(k+1)\&rsqb;}^T\};$

wherein E {. represents a mathematical expectation;

the measurement correction equation of the filter is

$\left\{\begin{array}{c}{K}_x(k+1)=P_x(k+1/k)H_x^T{\&lsqb;H_x{P}_x(k+1/k)H_x^T+R_x\&rsqb;}^{-1}\\ {\hat{X}}_x(k+1)={\stackrel{\&OverBar;}{X}}_x(k+1)+K_x(k+1)\&lsqb;{\mathrm{\&eta;}}_x(k+1)-H_x{\stackrel{\&OverBar;}{X}}_x(k+1)\&rsqb;\\ P_x(k+1)=\&lsqb;I_3-K_x(k+1)H_x\&rsqb;P_x(k+1/k)\end{array}\right.---\left(10\right)$

Wherein, K_x(k +1) is the filter gain of the filter at time k + 1; η_x(k +1) is measurement information at the time k + 1; i is₃Is a 3 × 3D identity matrix R_xIs a measurement error covariance matrix; the measurement matrix is

H_x＝[100](11)

When the measurement information does not exist, only the forecast equation (9) is operated, and when the measurement information exists, the forecast equation (9) and the measurement correction equation (10) are operated simultaneously to obtain o of the inertial coordinate system of the transmitting point_Fx_FTarget acceleration a of the shaft_txAn estimated value of (d);

step 3.3, designing an inertial coordinate system y of the transmitting point according to the step 3.2_FAxis and z_FThe target of the axis tracks the Kalman filter and obtains y of the inertial coordinate system of the transmitting point_FAxis and z_FTarget acceleration a of the shaft_tyAnd a_tzAn estimated value of (d);

step 3.4, target acceleration a_tx、a_tyAnd a_tzThe estimated value is projected to a sight line coordinate system from an inertial coordinate system of the transmitting point, and then o' y of the target acceleration in the sight line coordinate system is obtained₄And o' z₄Component a in the axial direction_tAnd a_tβ。

Other steps and parameters are the same as in one of the first to third embodiments.

The fifth concrete implementation mode:

calculating the view pitch angle q in step four of the present embodiment And line of sight yaw angle q_βThe method comprises the following specific steps:

the round point of the coordinate system measured by the seeker is at the gyration center o' of the seeker, and three axes of the round point are respectively parallel to the three axes of the round point without considering the installation error of the seeker and the flexible deformation of the installation position of the seekerOx of projectile coordinate system₁Axle, oy₁Axis and oz₁The axis, the seeker outputs the view pitch angle and view yaw angle △ q of the target-missile view relative to the seeker measurement coordinate system And △ q_β；

If the seeker measurement coordinate system is coincident with the missile coordinate system, the projection of the unit vector of the target-missile sight line in the seeker measurement coordinate system is

$\left\{\begin{array}{c}{c}_{1x}={\mathrm{cos\&Delta;q}}_{\mathrm{\&epsiv;}}{\mathrm{cos\&Delta;q}}_{\mathrm{\&beta;}}\\ c_{1y}={\mathrm{sin\&Delta;q}}_{\mathrm{\&epsiv;}}\\ c_{1z}=-{\mathrm{cos\&Delta;q}}_{\mathrm{\&epsiv;}}{\mathrm{sin\&Delta;q}}_{\mathrm{\&beta;}}\end{array}\right.---\left(12\right)$

Wherein, c_1x、c_1y、c_1zRespectively projecting unit vectors of the target-missile sight lines on three axes of a seeker measurement coordinate system;

and the projection of the unit vector of the target-missile sight line in the inertial coordinate system of the launching point is

$\left\{\begin{array}{c}{c}_{0x}={\mathrm{cosq}}_{\mathrm{\&epsiv;}}{\mathrm{cosq}}_{\mathrm{\&beta;}}\\ c_{0y}={\mathrm{sinq}}_{\mathrm{\&epsiv;}}\\ c_{0z}=-{\mathrm{cosq}}_{\mathrm{\&epsiv;}}{\mathrm{sinq}}_{\mathrm{\&beta;}}\end{array}\right.---\left(13\right)$

Wherein, c_0x、c_0y、c_0zRespectively projecting unit vectors of the target-missile sight lines on three axes of an inertial coordinate system of a launching point;

then

$\left[\begin{array}{c}{c}_{0x}\\ c_{0y}\\ c_{0z}\end{array}\right]=C_{F\&RightArrow;1}^T\left[\begin{array}{c}{c}_{1x}\\ c_{1y}\\ c_{1z}\end{array}\right]---\left(14\right)$

In the formula, C_F→1Is a transformation matrix from an inertial coordinate system of a transmitting point to a missile coordinate system,

$C_{F\&RightArrow;1}=\left[\begin{array}{ccc}\cos \mathrm{\&theta;}cos\mathrm{\&psi;}& \sin \mathrm{\&theta;}& -sin\mathrm{\&psi;}\cos \mathrm{\&theta;}\\ -\sin \mathrm{\&theta;}cos\mathrm{\&psi;}cos\mathrm{\&gamma;}+sin\mathrm{\&psi;}sin\mathrm{\&gamma;}& \cos \mathrm{\&theta;}cos\mathrm{\&gamma;}& \sin \mathrm{\&theta;}sin\mathrm{\&psi;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&psi;}sin\mathrm{\&gamma;}\\ \sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\sin \mathrm{\&gamma;}+sin\mathrm{\&psi;}\cos \mathrm{\&gamma;}& -\cos \mathrm{\&theta;}sin\mathrm{\&gamma;}& -\sin \mathrm{\&theta;}sin\mathrm{\&psi;}\sin \mathrm{\&gamma;}+cos\mathrm{\&psi;}cos\mathrm{\&gamma;}\end{array}\right]---\left(15\right)$

wherein,psi and gamma are respectively a pitch angle, a yaw angle and a rolling angle of the projectile body, and are measured and calculated by an inertial navigation system;

the view pitch angle and view yaw angle are calculated according to equation (13), i.e.

q ＝arcsinc_0y(16)

$q_{\mathrm{\&beta;}}=-arctan\frac{c_{0z}}{c_{0x}}---\left(17\right).$

Other steps and parameters are the same as in one of the first to fourth embodiments.

The sixth specific implementation mode:

the fifth step of the present embodiment is to calculate the view pitch angle rate and the view yaw angle rate between the target and the missile, specifically as follows:

step 5.1, the equation of the motion state of the pitching channel sight line is

${\stackrel{\&CenterDot;\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}=-\frac{2\stackrel{\&CenterDot;}{R}}{R}{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}+\frac{1}{R}\&lsqb;a_{t\mathrm{\&epsiv;}}-a_{\mathrm{\&epsiv;}}\&rsqb;---\left(18\right)$

Discretizing the formula (18) to obtain

$\left\{\begin{array}{c}{q}_{\mathrm{\&epsiv;}}(k+1)=q_{\mathrm{\&epsiv;}}\left(k\right)+\mathrm{\&Delta;}t{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}\left(k\right)\\ {\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}(k+1)=\&lsqb;1-\frac{2\stackrel{\&CenterDot;}{R}\left(k\right)}{R\left(k\right)}\mathrm{\&Delta;}t\&rsqb;{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}\left(k\right)+\frac{\mathrm{\&Delta;}t}{R\left(k\right)}\&lsqb;a_{t\mathrm{\&epsiv;}}\left(k\right)-a_{\mathrm{\&epsiv;}}\left(k\right)\&rsqb;\end{array}\right.---\left(19\right)$

Wherein k represents the kth sampling moment, and Δ t is the sampling period;

the measurement equation of the elevation channel line-of-sight angle is

$Z_{\mathrm{\&epsiv;}}\left(k\right)=q_{\mathrm{\&epsiv;}}\left(k\right)+v_{\mathrm{\&epsiv;}}\left(k\right)=H_{\mathrm{\&epsiv;}}{\left[\begin{array}{cc}{q}_{\mathrm{\&epsiv;}}\left(k\right)& {\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}\left(k\right)\end{array}\right]}^T+v_{\mathrm{\&epsiv;}}\left(k\right)---\left(20\right)$

Wherein H ＝[10]，ν (k) The measurement noise of the view line pitch angle is assumed to be zero;

is provided with $E\&lsqb;w_{\mathrm{\&epsiv;}}\left(k\right)w_{\mathrm{\&epsiv;}}^T\left(k\right)\&rsqb;=Q_{\mathrm{\&epsiv;}}\left(k\right),E\&lsqb;v_{\mathrm{\&epsiv;}}\left(k\right)v_{\mathrm{\&epsiv;}}^T\left(k\right)\&rsqb;=R_{\mathrm{\&epsiv;}}\left(k\right),$

$X_{\mathrm{\&epsiv;}}=\left[\begin{array}{c}{q}_{\mathrm{\&epsiv;}}\\ {\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}\end{array}\right],u_{\mathrm{\&epsiv;}}=\left[\begin{array}{c}0\\ a_{t\mathrm{\&epsiv;}}-a_{\mathrm{\&epsiv;}}\end{array}\right],{\mathrm{\&Phi;}}_{\mathrm{\&epsiv;}}=\left[\begin{array}{cc}1& \mathrm{\&Delta;}t\\ 0& 1-\mathrm{\&Delta;}t2\stackrel{\&CenterDot;}{R}/R\end{array}\right],B_{\mathrm{\&epsiv;}}=\left[\begin{array}{c}0\\ \mathrm{\&Delta;}t/R\end{array}\right],$

The pitch path Kalman filter is then

$\{\begin{array}{c}{\stackrel{\&OverBar;}{X}}_{\mathrm{\&epsiv;}}(k+1)={\mathrm{\&Phi;}}_{\mathrm{\&epsiv;}}\left(k\right){\hat{X}}_{\mathrm{\&epsiv;}}\left(k\right)+B_{\mathrm{\&epsiv;}}\left(k\right)u_{\mathrm{\&epsiv;}}\left(k\right)\\ P_{\mathrm{\&epsiv;}}(k+1/k)={\mathrm{\&Phi;}}_{\mathrm{\&epsiv;}}\left(k\right)P_{\mathrm{\&epsiv;}}\left(k\right){\mathrm{\&Phi;}}_{\mathrm{\&epsiv;}}^T\left(k\right)+Q_{\mathrm{\&epsiv;}}\left(k\right)\\ K_{\mathrm{\&epsiv;}}(k+1)=P_{\mathrm{\&epsiv;}}(k+1/k)H_{\mathrm{\&epsiv;}}^T{\&lsqb;H_{\mathrm{\&epsiv;}}{P}_{\mathrm{\&epsiv;}}(k+1/k)H_{\mathrm{\&epsiv;}}^T+R_{\mathrm{\&epsiv;}}(k+1)\&rsqb;}^{-1}\\ {\hat{X}}_{\mathrm{\&epsiv;}}(k+1)={\stackrel{\&OverBar;}{X}}_{\mathrm{\&epsiv;}}(k+1)+K_{\mathrm{\&epsiv;}}(k+1)\&lsqb;Z_{\mathrm{\&epsiv;}}(k+1)-H_{\mathrm{\&epsiv;}}{\stackrel{\&OverBar;}{X}}_{\mathrm{\&epsiv;}}(k+1)\&rsqb;\\ P_{\mathrm{\&epsiv;}}(k+1)=\&lsqb;I_2-K_{\mathrm{\&epsiv;}}(k+1)H\&rsqb;P_{\mathrm{\&epsiv;}}(k+1/k)\end{array}---\left(21\right)$

Wherein,andeach represents X Filter estimation and prediction estimation of, K (k +1) is the filter gain at the moment of the pitch channel Kalman filter k + 1; i is₂An identity matrix of dimension 2 × 2;

$P_{\mathrm{\&epsiv;}}(k+1/k)=E\{\&lsqb;X_{\mathrm{\&epsiv;}}(k+1)-{\stackrel{\&OverBar;}{X}}_{\mathrm{\&epsiv;}}(k+1)\&rsqb;{\&lsqb;X_{\mathrm{\&epsiv;}}(k+1)-{\stackrel{\&OverBar;}{X}}_{\mathrm{\&epsiv;}}(k+1)\&rsqb;}^T\},$

$P_{\mathrm{\&epsiv;}}(k+1)=E\{\&lsqb;X_{\mathrm{\&epsiv;}}(k+1)-{\hat{X}}_{\mathrm{\&epsiv;}}(k+1)\&rsqb;{\&lsqb;X_{\mathrm{\&epsiv;}}(k+1)-{\hat{X}}_{\mathrm{\&epsiv;}}(k+1)\&rsqb;}^T\};$

step 5.2, writing the discretization state equation of the visual line motion of the yaw channel

X_β(k+1)＝Φ_β(k)X_β(k)+Β_β(k)u_β(k)+w_β(k) (22) wherein, in the above-mentioned step, $X_{\mathrm{\&beta;}}=\left[\begin{array}{c}{q}_{\mathrm{\&beta;}}\\ {\stackrel{\&CenterDot;}{q}}_{\mathrm{\&beta;}}\end{array}\right],u_{\mathrm{\&beta;}}=\left[\begin{array}{c}0\\ a_{\mathrm{\&beta;}}-a_{t\mathrm{\&beta;}}\end{array}\right],{\mathrm{\&Phi;}}_{\mathrm{\&beta;}}=\left[\begin{array}{cc}1& \mathrm{\&Delta;}t\\ 0& 1-\mathrm{\&Delta;}t2\stackrel{\&CenterDot;}{R}/R\end{array}\right],B_{\mathrm{\&beta;}}=\left[\begin{array}{c}0\\ \mathrm{\&Delta;}t/R\end{array}\right];$

w_β(k) is a zero-mean random process and is,

the yaw line-of-sight angle measurement equation is

$Z_{\mathrm{\&beta;}}\left(k\right)=q_{\mathrm{\&beta;}}\left(k\right)+v_{\mathrm{\&beta;}}\left(k\right)=H_{\mathrm{\&beta;}}{\left[\begin{array}{cc}{q}_{\mathrm{\&beta;}}\left(k\right)& {\stackrel{\&CenterDot;}{q}}_{\mathrm{\&beta;}}\left(k\right)\end{array}\right]}^T+v_{\mathrm{\&beta;}}\left(k\right)---\left(23\right)$

Wherein H_β＝[10]，ν_β(k) Is the measurement noise of the elevation angle of the view, assuming that the mean value is zero, $E\&lsqb;v_{\mathrm{\&epsiv;}}\left(k\right)v_{\mathrm{\&epsiv;}}^T\left(k\right)\&rsqb;=R_{\mathrm{\&epsiv;}}\left(k\right);$

the yaw path Kalman filter is

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{X}}_{\mathrm{\&beta;}}(k+1)={\mathrm{\&Phi;}}_{\mathrm{\&beta;}}\left(k\right){\hat{X}}_{\mathrm{\&beta;}}\left(k\right)+B_{\mathrm{\&beta;}}\left(k\right)u_{\mathrm{\&beta;}}\left(k\right)\\ P_{\mathrm{\&beta;}}(k+1/k)={\mathrm{\&Phi;}}_{\mathrm{\&beta;}}\left(k\right)P_{\mathrm{\&beta;}}\left(k\right){\mathrm{\&Phi;}}_{\mathrm{\&beta;}}^T\left(k\right)+Q_{\mathrm{\&beta;}}\left(k\right)\\ K_{\mathrm{\&beta;}}(k+1)=P_{\mathrm{\&beta;}}(k+1/k)H_{\mathrm{\&beta;}}^T{\&lsqb;H_{\mathrm{\&beta;}}{P}_{\mathrm{\&beta;}}(k+1/k)H_{\mathrm{\&beta;}}^T+R_{\mathrm{\&beta;}}(k+1)\&rsqb;}^{-1}\\ {\hat{X}}_{\mathrm{\&beta;}}(k+1)={\stackrel{\&OverBar;}{X}}_{\mathrm{\&beta;}}(k+1)+K_{\mathrm{\&beta;}}(k+1)\&lsqb;Z_{\mathrm{\&beta;}}(k+1)-H_{\mathrm{\&beta;}}{\stackrel{\&OverBar;}{X}}_{\mathrm{\&beta;}}(k+1)\&rsqb;\\ P_{\mathrm{\&beta;}}(k+1)=\&lsqb;I_2-K_{\mathrm{\&beta;}}(k+1)H\&rsqb;P_{\mathrm{\&beta;}}(k+1/k)\end{array}\right.---\left(24\right)$

Wherein,andeach represents X_βFilter estimation and prediction estimation of, K_β(k +1) is the filter gain at time k +1 of the yaw path Kalman filter;

$P_{\mathrm{\&beta;}}(k+1/k)=E\{\&lsqb;X_{\mathrm{\&beta;}}(k+1)-{\stackrel{\&OverBar;}{X}}_{\mathrm{\&beta;}}(k+1)\&rsqb;{\&lsqb;X_{\mathrm{\&beta;}}(k+1)-{\stackrel{\&OverBar;}{X}}_{\mathrm{\&beta;}}(k+1)\&rsqb;}^T\},$

$P_{\mathrm{\&beta;}}(k+1)=E\{\&lsqb;X_{\mathrm{\&beta;}}(k+1)-{\hat{X}}_{\mathrm{\&beta;}}(k+1)\&rsqb;{\&lsqb;X_{\mathrm{\&beta;}}(k+1)-{\hat{X}}_{\mathrm{\&beta;}}(k+1)\&rsqb;}^T\}$

and 5.3, accurately solving the view-line pitch angle rate and the view-line yaw angle rate between the target and the missile according to the filters in the step 5.1 and the step 5.2.

Other steps and parameters are the same as in one of the first to fifth embodiments.

Examples

The following explains the effect of the present invention by the result of numerical simulation.

And carrying out full trajectory simulation on the process of intercepting the air high-speed maneuvering target by the foundation interception missile. The data updating period of the missile inertial navigation system is 2.5ms, the data updating period of the seeker is 10ms, and the filtering period and the guidance period of the Kalman filter are 10 ms.

In a line-of-sight angular rate Kalman filter, the initial value of the state estimation variance matrix is taken as

$P_{\mathrm{\&epsiv;}}\left(k\right)=\left[\begin{array}{cc}{10}^{-8}& 0\\ 0& {10}^{-8}\end{array}\right],P_{\mathrm{\&beta;}}\left(k\right)=\left[\begin{array}{cc}{10}^{-8}& 0\\ 0& {10}^{-8}\end{array}\right]$

Taking R from variance of measured noise₀(k)＝10^-6，R_β0(k)＝10^-6The dynamic noise variance matrix is taken as

$Q_{\mathrm{\&epsiv;}0}\left(k\right)=\left[\begin{array}{cc}{10}^{-8}& 0\\ 0& {10}^{-8}\end{array}\right],Q_{\mathrm{\&beta;}0}\left(k\right)=\left[\begin{array}{cc}{10}^{-8}& 0\\ 0& {10}^{-8}\end{array}\right]$

Initial estimate of filterAndtaken as the line of sight angle calculated from the seeker's beat 1 measurement,andcalculating by a geometric method: namely, it is

${\widehat{\stackrel{\&CenterDot;}{q}}}_{\mathrm{\&epsiv;}}\left(0\right)=\frac{({\mathrm{\&Delta;x}}^2+{\mathrm{\&Delta;z}}^2){\mathrm{\&Delta;V}}_y-\mathrm{\&Delta;}y({\mathrm{\&Delta;x\&Delta;V}}_x+{\mathrm{\&Delta;z\&Delta;V}}_z)}{({\mathrm{\&Delta;x}}^2+{\mathrm{\&Delta;y}}^2+{\mathrm{\&Delta;z}}^2)\sqrt{{\mathrm{\&Delta;x}}^2+{\mathrm{\&Delta;z}}^2}}---\left(25\right)$

${\widehat{\stackrel{\&CenterDot;}{q}}}_{\mathrm{\&beta;}}\left(0\right)=\frac{{\mathrm{\&Delta;z\&Delta;V}}_x-{\mathrm{\&Delta;x\&Delta;V}}_z}{{\mathrm{\&Delta;x}}^2+{\mathrm{\&Delta;z}}^2}---\left(26\right)$

For a target flying at an altitude of about 20km, a trajectory with an interception slant of about 90km is examined:

at 0s, in the inertial coordinate system of the launching point, the missile speed V_x＝0m/s,V_y＝0m/s,V_z0m/s, 0m for missile position x, 0m for y, 0m for z, and 0m for missile angular velocity ω_x＝0°/s,ω_y＝0°/s，ω_z0 DEG/s and attitude anglePsi-0 °, γ -0 °; target position x_t＝220km，y_t＝16.2km，z_t0km, target velocity V_xt＝-1499.113m/s，V_yt＝51.5776m/s，V_zt0m/s, target flying height H_tc19.983 km. The target is normally maneuvered laterally, the maneuvering amplitude a_t＝g。

Fig. 4 and 5 are a simulation effect diagram of a longitudinal flight trajectory of a missile interception target and a simulation effect diagram of a lateral flight trajectory of the missile interception target, respectively. The ordinate H in fig. 5 represents the flight height in km. The target height is calculated by the formula H_t＝r_tI-R_eWhereinRepresenting the center-to-center distance, x, of the target_tI＝x_t，y_tI＝y_t+R_e，z_tI＝z_tRepresenting the coordinates of the target in the geocentric inertial frame shown in FIG. 1, R_eIs the radius of the earth; the missile height calculation formula is H ═ r_I-R_eWhereinRepresenting the centre-to-earth distance, x, of the missile_I＝x，y_I＝y+R_e，z_IAnd z represents the coordinates of the missile in the earth-centered inertial coordinate system shown in fig. 1. Under the trajectory, the terminal interception time is 88.138s, the interception slant distance is 92.494km, the interception height is 19.9829km, and the terminal miss distance is 0.1102 m.

FIG. 6 is a view of a simulation effect of a target-missile view angle rate, and FIG. 7 is a view of a simulation effect of a target-missile view yaw rate. In fig. 6 and 7, true represents the true value of the line-of-sight angular rate, Kal1 represents the maneuvering target tracking line-of-sight angular rate Kalman filter proposed herein, Kal0 represents the original non-maneuvering target tracking line-of-sight angular rate Kalman filter, and fig. 6 and 7 show that the convergence rate of the maneuvering target tracking line-of-sight angular rate Kalman filter is fast, the estimated value after 1s is available, and the estimated value thereof can well track the true value of the line-of-sight angular rate. Fig. 6 and 7 also show that the estimation result of the maneuvering target tracking line-of-sight angular rate Kalman filter is obviously better than that of the original non-maneuvering target tracking line-of-sight angular rate Kalman filter, and the steady-state precision is improved by about 0.03 degrees/s to the maximum.

The results of the target tracking filter estimating the components of the target maneuvering acceleration on the three axes of the launching point inertial coordinate system are plotted in fig. 8-10, and it can be seen from fig. 8-10 that the error of the estimation on the target acceleration is less than 2m/s²。

Claims (6)

1. A method of estimating angular rate of sight for a motorized target, comprising the steps of:

step one, measuring and calculating a target-missile relative distance R and a target-missile relative speed

Step two, measuring and calculating the acceleration of the missile in a sight line coordinate system o' x₄y₄z₄O' y of₄And o' z₄Component a in the axial direction And a_β；

Step three, estimating the target acceleration in a sight line coordinate system o' x₄y₄z₄O' y of₄And o' z₄Component a in the axial direction_tAnd a_tβ；

Step four, calculating the view pitch angle q And line of sight yaw angle q_β；

Step five, the relative distance R between the target and the missile and the relative speed of the target and the missileComponent of missile acceleration a And a_βTarget acceleration estimation result a_tAnd a_tβAnd the calculation result q of the view pitch angle and the view yaw angle And q is_βAnd respectively substituting the angular rate of the pitching channel line of sight into a Kalman filter and the angular rate of the yawing channel line of sight into the Kalman filter of the guided missile, thereby accurately solving the line of sight pitch angle rate and the line of sight yaw rate between the target and the guided missile.

2. The method of claim 1, wherein said step of estimating target-missile relative distance R and target-missile relative velocityThe method comprises the following specific steps:

defining a geocentric inertial coordinate system: center of earth inertial coordinate system o_Ix_Iy_Iz_IOrigin o of_ILocated on the center of the earth o_Ix_IThe axis lying in the equatorial plane and pointing to a certain star, o_Iz_IThe axis pointing in the direction of the north pole, o_Iy_IDetermining according to the right-hand rule;

defining an inertial coordinate system of the emission point: setting an inertial coordinate system o of a transmitting point_Fx_Fy_Fz_FThe missile is fixed relative to the earth center inertial coordinate system at the moment of missile launching, and the origin of the inertial coordinate system of the launching point is the launchingPoint o_F，o_Fy_FAxial plumb upward, o_Fx_FShaft and o_Fz_FThe axes lying in a horizontal plane and having their orientation selected as desired, o_Fx_Fy_FIs a shooting plane;

λ₀and α_FRespectively the latitude, longitude and transmitting azimuth of the transmitting point relative to the geocentric inertial coordinate system;

defining an end guidance initial sight line coordinate system: o₀x₀y₀z₀For final guidance of the initial line-of-sight coordinate system, o₀The missile seeker is positioned on the rotation center of the missile seeker at the final guidance initial moment; o₀x₀The initial sight direction is the pointing target is positive; o₀y₀Is located at and contains o₀x₀In the vertical plane of (a), perpendicular to o₀x₀Pointing upward is positive; o₀z₀Determining according to the right-hand rule; during the whole final guidance process, o₀x₀y₀z₀The coordinate system is relative to the inertial coordinate system o of the transmitting point_Fx_Fy_Fz_FCuring is still; q. q.s₀And q is_β0Respectively an initial sight line pitch angle and a sight line yaw angle formed by the sight line relative to an inertial coordinate system of the emission point;

the position of the missile in the inertial coordinate system of the launching point is (x, y, z), and the projection of the velocity of the missile in the inertial coordinate system is (V)_x、V_y、V_z)；

The position of the target in the inertial coordinate system of the emitting point is (x)_t、y_t、z_t) The projection of its velocity in the coordinate system is (V)_tx、V_ty、V_tz)；

According to △ x ═ x-x_t，△y＝y-y_t，△z＝z-z_t，△V_x＝V_x-V_tx，△V_y＝V_y-V_ty，△V_z＝V_z-V_tzCalculating the target-missile relative distanceAnd target-missile relative velocity $\stackrel{\&CenterDot;}{R}=({\mathrm{\&Delta;x\&Delta;V}}_x+{\mathrm{\&Delta;y\&Delta;V}}_y+{\mathrm{\&Delta;z\&Delta;V}}_z)/R;$ In the final guidance process, there is always R>0， $\stackrel{\&CenterDot;}{R}<0.$

3. The method of estimating line-of-sight angular rate for a maneuvering target according to claim 2, characterized by the step two of estimating the missile acceleration in the line-of-sight coordinate system o' x₄y₄z₄O' y of₄And o' z₄Component a in the axial direction And a_βThe method comprises the following specific steps:

defining a line-of-sight coordinate system: line of sight coordinate system o' x₄y₄z₄The origin o' is positioned at the rotation center of the seeker; o' x₄The axis is consistent with the sight of the target missile at the current moment, and the direction from the rotation center of the seeker to the target is positive; o' y₄The axis being located containing o' x₄Within the plumb plane of the shaft, with o' x₄The axis is vertical, and the pointing direction is positive; o' z₄The axis is determined according to the right-hand rule; converting the initial sight line coordinate system of end guidance into a sight line coordinate system to define a sight line pitch angle q And line of sight yaw angle q_β；

Defining a projectile coordinate system; missile projectile body coordinate system ox₁y₁z₁Is located on the missile centroid; ox₁The axis is coincident with the longitudinal axis of the projectile body, and the pointing head is positive; oy₁With axis in the longitudinal symmetry plane of the projectile body, perpendicular to ox₁A shaft pointing upward as positive; oz₁The axial direction is determined according to the right-hand rule;

acceleration output by accelerometer on missileIs along the body coordinate system ox₁y₁z₁Projected acceleration from accelerometer on missileProjected to a line of sight coordinate system o' x₄y₄z₄O' y of₄And o' z₄Obtaining missile sight line coordinate system o' x in axial direction₄y₄z₄O' y of₄And o' z₄Acceleration a in the axial direction And a_β。

4. The method of claim 3, wherein the step three of estimating the acceleration of the target is performed in a visual coordinate system o' x₄y₄z₄O' y of₄And o' z₄Component a in the axial direction_tAnd a_tβThe method comprises the following specific steps:

step 3.1, respectively describing the target acceleration component a by using Singer models on three axes of the inertial coordinate system of the launching point_tx、a_tyAnd a_tz：

${\stackrel{\&CenterDot;}{a}}_{tx}=-{\mathrm{\&lambda;}}_x{a}_{tx}+w_x$

${\stackrel{\&CenterDot;}{a}}_{ty}=-{\mathrm{\&lambda;}}_y{a}_{ty}+w_y$

${\stackrel{\&CenterDot;}{a}}_{tz}=-{\mathrm{\&lambda;}}_z{a}_{tz}+w_z$

Wherein λ is_x、λ_yAnd λ_zRespectively representing the reciprocal of a target maneuvering time constant in the directions of three axes of an inertial coordinate system of the launching point; w is a_x、w_yAnd w_zRespectively representing zero mean Gaussian white noise on three axes;anddenotes a_tx、a_tyAnd a_tzThe upper band of the parameter, each representing a derivative of the parameter;

three sets of motion equations are established

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_t=V_{tx}\\ {\stackrel{\&CenterDot;}{V}}_{tx}=a_{tx}\\ {\stackrel{\&CenterDot;}{a}}_{tx}=-{\mathrm{\&lambda;}}_x{a}_{tx}+w_x\end{array}\right.---\left(1\right)$

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{y}}_t=V_{ty}\\ {\stackrel{\&CenterDot;}{V}}_{ty}=a_{ty}\\ {\stackrel{\&CenterDot;}{a}}_{ty}=-{\mathrm{\&lambda;}}_y{a}_{ty}+w_y\end{array}\right.---\left(2\right)$

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{z}}_t=V_{tz}\\ {\stackrel{\&CenterDot;}{V}}_{tz}=a_{tz}\\ {\stackrel{\&CenterDot;}{a}}_{tz}=-{\mathrm{\&lambda;}}_z{a}_{tz}+w_z\end{array}\right.---\left(3\right)$

Step 3.2, with respect to equation (1), a target acceleration component a is defined_txO in the inertial frame of the emission point_Fx_FState variable X of shaft_x＝[x_tV_txa_tx]^TThen the following equation of state is obtained

${\stackrel{\&CenterDot;}{X}}_x=F_x{X}_x---\left(4\right)$

Wherein,

$F_x=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& -{\mathrm{\&lambda;}}_x\end{array}\right]---\left(5\right)$

discretizing the formula (5) by a sampling period delta t to obtain

${\mathrm{\&Phi;}}_x=\exp \left(F_x\mathrm{\&Delta;}t\right)=\left[\begin{array}{ccc}1& \mathrm{\&Delta;}t& 1/{\mathrm{\&lambda;}}_x^2(e^{-{\mathrm{\&lambda;}}_x\mathrm{\&Delta;}t}+{\mathrm{\&lambda;}}_x\mathrm{\&Delta;}t-1)\\ 0& 1& 1/{\mathrm{\&lambda;}}_x(1-e^{-{\mathrm{\&lambda;}}_x\mathrm{\&Delta;}t})\\ 0& 0& e^{-{\mathrm{\&lambda;}}_x\mathrm{\&Delta;}t}\end{array}\right]---\left(6\right)$

The discretized equation of state is

X_x(k+1)＝Φ_xX_x(k)(7)

Wherein k represents the kth sampling instant;

the measurement equation is

η_x(k)＝x_t(k)(8)

According to the model as formula (1), the following emission point inertial coordinate system x is designed_FA Kalman filter for target tracking of axis with prediction equation of

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{X}}_x(k+1)={\mathrm{\&Phi;}}_x{\hat{X}}_x\left(k\right)\\ P_x(k+1/k)={\mathrm{\&Phi;}}_x{P}_x\left(k\right){\mathrm{\&Phi;}}_x^T+Q_x\end{array}\right.---\left(9\right)$

Wherein,andeach represents X_xFiltering estimation and prediction estimation of (1); q_xPredicting an error covariance matrix for the model;

$P_x(k+1/k)=E\{\&lsqb;X_x(k+1)-{\stackrel{\&OverBar;}{X}}_x(k+1)\&rsqb;{\&lsqb;X_x(k+1)-{\stackrel{\&OverBar;}{X}}_x(k+1)\&rsqb;}^T\},$

$P_x(k+1)=E\{\&lsqb;X_x(k+1)-{\hat{X}}_x(k+1)\&rsqb;{\&lsqb;X_x(k+1)-{\hat{X}}_x(k+1)\&rsqb;}^T\};$

wherein E {. represents a mathematical expectation;

the measurement correction equation of the filter is

$\left\{\begin{array}{c}{K}_x(k+1)=P_x(k+1/k)H_x^T{\&lsqb;H_x{P}_x(k+1/k)H_x^T+R_x\&rsqb;}^{-1}\\ {\hat{X}}_x(k+1)={\stackrel{\&OverBar;}{X}}_x(k+1)+K_x(k+1)\&lsqb;{\mathrm{\&eta;}}_x(k+1)-H_x{\stackrel{\&OverBar;}{X}}_x(k+1)\&rsqb;\\ P_x(k+1)=\&lsqb;I_3-K_x(k-1)H_x\&rsqb;P_x(k+1/k)\end{array}\right.---\left(10\right)$

Wherein, K_x(k +1) is the filter gain of the filter at time k + 1; η_x(k +1) is measurement information at the time k + 1; i is₃Is a 3 × 3D identity matrix R_xIs a measurement error covariance matrix; the measurement matrix is

H_x＝[100](11)

When the measurement information does not exist, only the forecast equation (9) is operated, and when the measurement information exists, the forecast equation (9) and the measurement correction equation (10) are operated simultaneously to obtain o of the inertial coordinate system of the transmitting point_Fx_FTarget acceleration a of the shaft_txAn estimated value of (d);

step 3.3, designing an inertial coordinate system y of the transmitting point according to the step 3.2_FAxis and z_FThe target of the axis tracks the Kalman filter and obtains y of the inertial coordinate system of the transmitting point_FAxis and z_FTarget acceleration a of the shaft_tyAnd a_tzAn estimated value of (d);

step 3.4, target acceleration a_tx、a_tyAnd a_tzThe estimated value is projected to a sight line coordinate system from an inertial coordinate system of the transmitting point, and then o' y of the target acceleration in the sight line coordinate system is obtained₄And o' z₄Component a in the axial direction_tAnd a_tβ。

5. The method of claim 4, wherein said step four of calculating the elevation of view q And line of sight yaw angle q_βThe method comprises the following specific steps:

the round point of the seeker measuring coordinate system is positioned at the gyration center o' of the seeker, and three axes of the round point are respectively parallel to the ox of the missile coordinate system₁Axle, oy₁Axis and oz₁The axis, the seeker outputs the view pitch angle and view yaw angle △ q of the target-missile view relative to the seeker measurement coordinate system And △ q_β；

If the seeker measurement coordinate system is coincident with the missile coordinate system, the projection of the unit vector of the target-missile sight line in the seeker measurement coordinate system is

$\left\{\begin{array}{c}{c}_{1x}={\mathrm{cos\&Delta;q}}_{\mathrm{\&epsiv;}}{\mathrm{cos\&Delta;q}}_{\mathrm{\&beta;}}\\ c_{1y}={\mathrm{sin\&Delta;q}}_{\mathrm{\&epsiv;}}\\ c_{1z}=-{\mathrm{cos\&Delta;q}}_{\mathrm{\&epsiv;}}{\mathrm{sin\&Delta;q}}_{\mathrm{\&beta;}}\end{array}\right.---\left(12\right)$

Wherein, c_1x、c_1y、c_1zRespectively projecting unit vectors of the target-missile sight lines on three axes of a seeker measurement coordinate system;

and the projection of the unit vector of the target-missile sight line in the inertial coordinate system of the launching point is

$\left\{\begin{array}{c}{c}_{0x}={\mathrm{cosq}}_{\mathrm{\&epsiv;}}{\mathrm{cosq}}_{\mathrm{\&beta;}}\\ c_{0y}={\mathrm{sinq}}_{\mathrm{\&epsiv;}}\\ c_{0z}=-{\mathrm{cosq}}_{\mathrm{\&epsiv;}}{\mathrm{sinq}}_{\mathrm{\&beta;}}\end{array}\right.---\left(13\right)$

Wherein, c_0x、c_0y、c_0zRespectively projecting unit vectors of the target-missile sight lines on three axes of an inertial coordinate system of a launching point;

then

$\left[\begin{array}{c}{c}_{0x}\\ c_{0y}\\ c_{0z}\end{array}\right]=C_{F\&RightArrow;1}^T\left[\begin{array}{c}{c}_{1x}\\ c_{1y}\\ c_{1z}\end{array}\right]---\left(14\right)$

In the formula, C_F→1Is a transformation matrix from an inertial coordinate system of a transmitting point to a missile coordinate system,

$C_{F\&RightArrow;1}=\left[\begin{array}{ccc}\cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&theta;}& -\sin \mathrm{\&psi;}\cos \mathrm{\&theta;}\\ -\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&psi;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&theta;}\cos \mathrm{\&gamma;}& \sin \mathrm{\&theta;}\sin \mathrm{\&psi;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&psi;}\sin \mathrm{\&gamma;}\\ \sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&psi;}\cos \mathrm{\&gamma;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}& -\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&psi;}\cos \mathrm{\&gamma;}\end{array}\right]---\left(15\right)$

wherein, theta, psi and gamma are respectively a pitch angle, a yaw angle and a roll angle of the projectile body;

the view pitch angle and view yaw angle are calculated according to equation (13), i.e.

$q_{\mathrm{\&epsiv;}}={\mathrm{arcsinc}}_{0y}---\left(16\right)$

$q_{\mathrm{\&beta;}}=-arctan\frac{c_{0z}}{c_{0x}}---\left(17\right).$

6. The method of claim 5, wherein the step five of finding the line-of-sight pitch angle rate and the line-of-sight yaw rate between the target and the missile comprises the following steps:

step 5.1, the equation of the motion state of the pitching channel sight line is

${\stackrel{\&CenterDot;\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}=-\frac{2\stackrel{\&CenterDot;}{R}}{R}{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}+\frac{1}{R}\&lsqb;a_{t\mathrm{\&epsiv;}}-a_{\mathrm{\&epsiv;}}\&rsqb;---\left(18\right)$

Discretizing the formula (18) to obtain

$\left\{\begin{array}{c}{q}_{\mathrm{\&epsiv;}}(k+1)=q_{\mathrm{\&epsiv;}}\left(k\right)+\mathrm{\&Delta;}t{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}\left(k\right)\\ {\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}(k+1)=\&lsqb;1-\frac{2\stackrel{\&CenterDot;}{R}\left(k\right)}{R\left(k\right)}\mathrm{\&Delta;}t\&rsqb;{\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}\left(k\right)+\frac{\mathrm{\&Delta;}t}{R\left(k\right)}\&lsqb;a_{t\mathrm{\&epsiv;}}\left(k\right)-a_{\mathrm{\&epsiv;}}\left(k\right)\&rsqb;\end{array}\right.---\left(19\right)$

Wherein k represents the kth sampling moment, and Δ t is the sampling period;

the measurement equation of the elevation channel line-of-sight angle is

$Z_{\mathrm{\&epsiv;}}\left(k\right)=q_{\mathrm{\&epsiv;}}\left(k\right)+v_{\mathrm{\&epsiv;}}\left(k\right)=H_{\mathrm{\&epsiv;}}{\left[\begin{array}{cc}{q}_{\mathrm{\&epsiv;}}\left(k\right)& {\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}\left(k\right)\end{array}\right]}^T+v_{\mathrm{\&epsiv;}}\left(k\right)---\left(20\right)$

Wherein H ＝[10]，ν (k) The measurement noise of the view line pitch angle is assumed to be zero;

is provided with $E\&lsqb;w_{\mathrm{\&epsiv;}}\left(k\right)w_{\mathrm{\&epsiv;}}^T\left(k\right)\&rsqb;=Q_{\mathrm{\&epsiv;}}\left(k\right),$ $E\&lsqb;v_{\mathrm{\&epsiv;}}\left(k\right)v_{\mathrm{\&epsiv;}}^T\left(k\right)\&rsqb;=R_{\mathrm{\&epsiv;}}\left(k\right),$

$X_{\mathrm{\&epsiv;}}=\left[\begin{array}{c}{q}_{\mathrm{\&epsiv;}}\\ {\stackrel{\&CenterDot;}{q}}_{\mathrm{\&epsiv;}}\end{array}\right],$ $u_{\mathrm{\&epsiv;}}=\left[\begin{array}{c}0\\ a_{t\mathrm{\&epsiv;}}-a_{\mathrm{\&epsiv;}}\end{array}\right],$ ${\mathrm{\&Phi;}}_{\mathrm{\&epsiv;}}=\left[\begin{array}{cc}1& \mathrm{\&Delta;}t\\ 0& 1-\mathrm{\&Delta;}t2\stackrel{\&CenterDot;}{R}/R\end{array}\right],$ $B_{\mathrm{\&epsiv;}}=\left[\begin{array}{c}0\\ \mathrm{\&Delta;}t/R\end{array}\right],$

The pitch path Kalman filter is then

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{X}}_{\mathrm{\&epsiv;}}(k+1)={\mathrm{\&Phi;}}_{\mathrm{\&epsiv;}}\left(k\right){\hat{X}}_{\mathrm{\&epsiv;}}\left(k\right)+B_{\mathrm{\&epsiv;}}\left(k\right)u_{\mathrm{\&epsiv;}}\left(k\right)\\ P_{\mathrm{\&epsiv;}}(k+1/k)={\mathrm{\&Phi;}}_{\mathrm{\&epsiv;}}\left(k\right)P_{\mathrm{\&epsiv;}}\left(k\right){\mathrm{\&Phi;}}_{\mathrm{\&epsiv;}}^T\left(k\right)+Q_{\mathrm{\&epsiv;}}\left(k\right)\\ K_{\mathrm{\&epsiv;}}(k+1)=P_{\mathrm{\&epsiv;}}(k+1/k)H_{\mathrm{\&epsiv;}}^T{\&lsqb;H_{\mathrm{\&epsiv;}}{P}_{\mathrm{\&epsiv;}}(k+1/k)H_{\mathrm{\&epsiv;}}^T+R_{\mathrm{\&epsiv;}}(k+1)\&rsqb;}^{-1}\\ {\hat{X}}_{\mathrm{\&epsiv;}}(k+1)={\stackrel{\&OverBar;}{X}}_{\mathrm{\&epsiv;}}(k+1)+K_{\mathrm{\&epsiv;}}(k+1)\&lsqb;Z_{\mathrm{\&epsiv;}}(k+1)-H_{\mathrm{\&epsiv;}}{\stackrel{\&OverBar;}{X}}_{\mathrm{\&epsiv;}}(k+1)\&rsqb;\\ P_{\mathrm{\&epsiv;}}(k+1)=\&lsqb;I_2-K_{\mathrm{\&epsiv;}}(k+1)H\&rsqb;P_{\mathrm{\&epsiv;}}(k+1/k)\end{array}\right.---\left(21\right)$

Wherein,andeach represents X Filter estimation and prediction estimation of, K (k +1) is the time of pitch path Kalman filter k +1A filter gain; i is₂An identity matrix of dimension 2 × 2;

$P_{\mathrm{\&epsiv;}}(k+1/k)=E\{\&lsqb;X_{\mathrm{\&epsiv;}}(k+1)-{\stackrel{\&OverBar;}{X}}_{\mathrm{\&epsiv;}}(k+1)\&rsqb;{\&lsqb;X_{\mathrm{\&epsiv;}}(k+1)-{\stackrel{\&OverBar;}{X}}_{\mathrm{\&epsiv;}}(k+1)\&rsqb;}^T\},$

$P_{\mathrm{\&epsiv;}}(k+1)=E\{\&lsqb;X_{\mathrm{\&epsiv;}}(k+1)-{\hat{X}}_{\mathrm{\&epsiv;}}(k+1)\&rsqb;{\&lsqb;X_{\mathrm{\&epsiv;}}(k+1)-{\hat{X}}_{\mathrm{\&epsiv;}}(k+1)\&rsqb;}^T\};$

step 5.2, writing the discretization state equation of the visual line motion of the yaw channel

X_β(k+1)＝Φ_β(k)X_β(k)+Β_β(k)u_β(k)+w_β(k)(22)

Wherein, $X_{\mathrm{\&beta;}}=\left[\begin{array}{c}{q}_{\mathrm{\&beta;}}\\ {\stackrel{\&CenterDot;}{q}}_{\mathrm{\&beta;}}\end{array}\right],$ $u_{\mathrm{\&beta;}}=\left[\begin{array}{c}0\\ a_{\mathrm{\&beta;}}-a_{t\mathrm{\&beta;}}\end{array}\right],$ ${\mathrm{\&Phi;}}_{\mathrm{\&beta;}}=\left[\begin{array}{cc}1& \mathrm{\&Delta;}t\\ 0& 1-\mathrm{\&Delta;}t2\stackrel{\&CenterDot;}{R}/R\end{array}\right],$ $B_{\mathrm{\&beta;}}=\left[\begin{array}{c}0\\ \mathrm{\&Delta;}t/R\end{array}\right];$

w_β(k) is a zero-mean random process and is, $E\&lsqb;w_{\mathrm{\&beta;}}\left(k\right)w_{\mathrm{\&beta;}}^T\left(k\right)\&rsqb;=Q_{\mathrm{\&beta;}}\left(k\right);$

the yaw line-of-sight angle measurement equation is

$Z_{\mathrm{\&beta;}}\left(k\right)=q_{\mathrm{\&beta;}}\left(k\right)+v_{\mathrm{\&beta;}}\left(k\right)=H_{\mathrm{\&beta;}}{\left[\begin{array}{cc}{q}_{\mathrm{\&beta;}}\left(k\right)& {\stackrel{\&CenterDot;}{q}}_{\mathrm{\&beta;}}\left(k\right)\end{array}\right]}^T+v_{\mathrm{\&beta;}}\left(k\right)---\left(23\right)$

Wherein H_β＝[10]，ν_β(k) Is the measurement noise of the elevation angle of the view, assuming that the mean value is zero, $E\&lsqb;v_{\mathrm{\&epsiv;}}\left(k\right)v_{\mathrm{\&epsiv;}}^T\left(k\right)\&rsqb;=R_{\mathrm{\&epsiv;}}\left(k\right);$

the yaw path Kalman filter is

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{X}}_{\mathrm{\&beta;}}(k+1)={\mathrm{\&Phi;}}_{\mathrm{\&beta;}}\left(k\right){\hat{X}}_{\mathrm{\&beta;}}\left(k\right)+B_{\mathrm{\&beta;}}\left(k\right)u_{\mathrm{\&beta;}}\left(k\right)\\ P_{\mathrm{\&beta;}}(k+1/k)={\mathrm{\&Phi;}}_{\mathrm{\&beta;}}\left(k\right)P_{\mathrm{\&beta;}}\left(k\right){\mathrm{\&Phi;}}_{\mathrm{\&beta;}}^T\left(k\right)+Q_{\mathrm{\&beta;}}\left(k\right)\\ K_{\mathrm{\&beta;}}(k+1)=P_{\mathrm{\&beta;}}(k+1/k)H_{\mathrm{\&beta;}}^T{\&lsqb;H_{\mathrm{\&beta;}}{P}_{\mathrm{\&beta;}}(k+1/k)H_{\mathrm{\&beta;}}^T+R_{\mathrm{\&beta;}}(k+1)\&rsqb;}^{-1}\\ {\hat{X}}_{\mathrm{\&beta;}}(k+1)={\stackrel{\&OverBar;}{X}}_{\mathrm{\&beta;}}(k+1)+K_{\mathrm{\&beta;}}(k+1)\&lsqb;Z_{\mathrm{\&beta;}}(k+1)-H_{\mathrm{\&beta;}}{\stackrel{\&OverBar;}{X}}_{\mathrm{\&beta;}}(k+1)\&rsqb;\\ P_{\mathrm{\&beta;}}(k+1)=\&lsqb;I_2-K_{\mathrm{\&beta;}}(k+1)H\&rsqb;P_{\mathrm{\&beta;}}(k+1/k)\end{array}\right.---\left(24\right)$

Wherein,andeach represents X_βFilter estimation and prediction estimation of, K_β(k +1) is the filter gain at time k +1 of the yaw path Kalman filter;

$P_{\mathrm{\&beta;}}(k+1/k)=E\{\&lsqb;X_{\mathrm{\&beta;}}(k+1)-{\stackrel{\&OverBar;}{X}}_{\mathrm{\&beta;}}(k+1)\&rsqb;{\&lsqb;X_{\mathrm{\&beta;}}(k+1)-{\stackrel{\&OverBar;}{X}}_{\mathrm{\&beta;}}(k+1)\&rsqb;}^T\},$

$P_{\mathrm{\&beta;}}(k+1)=E\{\&lsqb;X_{\mathrm{\&beta;}}(k+1)-{\hat{X}}_{\mathrm{\&beta;}}(k+1)\&rsqb;{\&lsqb;X_{\mathrm{\&beta;}}(k+1)-{\hat{X}}_{\mathrm{\&beta;}}(k+1)\&rsqb;}^T\}$

and 5.3, accurately solving the view-line pitch angle rate and the view-line yaw angle rate between the target and the missile according to the filters in the step 5.1 and the step 5.2.