ROBUST ANGLE ONLY NINE STATE TARGET STATE ESTIMATOR (TSE)

Abstract

The present system estimates target motion with nonzero acceleration (including maneuvering uncertainty) using angle only (AO) measurements. The present approach employs a mixed coordinate system framework by combining modified spherical coordinate (MSC) system and Reference Cartesian Coordinate (RCC) system to keep accurate information flowing from one frame to the other while eliminating the numerical sensitivity of the angle measurements to the TSE vector. This integrated coordinate systems framework is achieved due to the state vector information of two frames (RCC and MSC) is effectively preserved between processing cycles and state vector transformation steps. The AO TSE architecture and processing schemes are applicable to a wide class of passive sensors. The mixed coordinate system provides robust real-time slant range estimation in a bootstrap fashion, thus turning passive AO measurements into equivalent active sensor measurements with built-in recursive range information but with greatly improved TSE accuracy.

Claims

1. An angle only (AO) target tracking and estimation method, comprising: updating angle only (AO) measurements in a modified spherical coordinate (MSC) system from a single passive sensor, wherein measurement updating uses a nine state matrix accounting for position, velocity, and acceleration each in 3 axes to address target maneuvering uncertainty; transforming data from the modified spherical coordinate (MSC) system to a reference Cartesian coordinate (RCC) system; time updating in the reference Cartesian coordinate (RCC) system; transforming data from the reference Cartesian coordinate (RCC) system to the modified spherical coordinate (MSC) system; and calculating the angle only (AO) measurements for a plurality of targets at a sensor interface level for use in guiding a projectile to each of the plurality of targets.

2. The angle only (AO) target tracking and estimation method according to claim 1, wherein measuring updating follows: .sub.k=y.sub.k C{circumflex over (z)}.sub.k|k1, where C=[0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0] is derivative free, {circumflex over (z)}.sub.k|k={circumflex over (z)}.sub.k|k1+K.sub.k, {circumflex over (P)}.sub.k|k={circumflex over (P)}.sub.k|k1KC{circumflex over (P)}.sub.k|k1, K={circumflex over (P)}.sub.k|k1C.sup.TS.sup.1, and S=C{circumflex over (P)}.sub.k|k1C.sup.T+R.sub.k, where R.sub.k is the sensor measurements noise covariance matrix.

3. The angle only (AO) target tracking and estimation method according to claim 1, wherein a steady state 3-D position error in three axes is less than 1 meter.

4. The angle only (AO) target tracking and estimation method according to claim 1, wherein the method is performed on-board a projectile using a single passive sensor.

5. The angle only (AO) target tracking and estimation method according to claim 4, wherein the single passive sensor is configured to track multiple targets.

6. The angle only (AO) target tracking and estimation method according to claim 5, wherein the multiple targets are in motion.

7. The angle only (AO) target tracking and estimation method according to claim 1, wherein transforming data from the modified spherical coordinate (MSC) system to the reference Cartesian coordinate (RCC) system follows: {circumflex over (x)}.sub.k1|k1=f.sub.x({circumflex over (z)}.sub.k1|k1).

8. The angle only (AO) target tracking and estimation method according to claim 1, wherein transforming data from the reference Cartesian coordinate (RCC) system to the modified spherical coordinate (MSC) system follows: {circumflex over (z)}.sub.k|k1=f.sub.z({circumflex over (x)}.sub.k|k1).

9. The angle only (AO) target tracking and estimation method according to claim 1, wherein time updating in the reference Cartesian coordinate (RCC) system follows: {circumflex over (x)}.sub.k|k1=A{circumflex over (x)}.sub.k1|k1+B.sub.ww.sub.k+B.sub.uu.sub.k where B.sub.u=B.sub.w and A=a nine state extended Kaufman filter (EKF) target state estimator (TSE) where $\left[\begin{array}{c}x\left(n+1\right)\\ y\left(n+1\right)\\ z\left(n+1\right)\\ v_x\left(n+1\right)\\ v_y\left(n+1\right)\\ v_z\left(n+1\right)\\ a_x\left(n+1\right)\\ a_y\left(n+1\right)\\ a_z\left(n+1\right)\end{array}\right]=\left[\begin{array}{ccccccccc}1& 0& 0& t& 0& 0& 0.5.Math.t^2& 0& 0\\ 0& 1& 0& 0& t& 0& 0& 0.5.Math.t^2& 0\\ 0& 0& 1& 0& 0& t& 0& 0& 0.5.Math.t^2\\ 0& 0& 0& 1& 0& 0& t& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& t& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& t\\ 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right].Math.\left[\begin{array}{c}x\left(n\right)\\ y\left(n\right)\\ z\left(n\right)\\ v_x\left(n\right)\\ v_y\left(n\right)\\ v_z\left(n\right)\\ a_x\left(n\right)\\ a_y\left(n\right)\\ a_z\left(n\right)\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ \frac{t}{m}& 0& 0\\ 0& \frac{t}{m}& 0\\ 0& 0& \frac{t}{m}\end{array}\right]\left[\begin{array}{c}{F}_x\\ F_y\\ F_z\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ \frac{t}{m}& 0& 0\\ 0& \frac{t}{m}& 0\\ 0& 0& \frac{t}{m}\end{array}\right]\left[\begin{array}{c}{F}_{\mathrm{xd}}\\ F_{y.Math..Math.d}\\ F_{\mathrm{zd}}\end{array}\right]$ {circumflex over (P)}.sub.k|k1=.sub.k|k1{circumflex over (P)}.sub.k1|k1.sub.k|k1+Q.sub.k1.sup.msc, where Q.sub.k1 is expressed in RCC, and the rcc state vector is renamed, {circumflex over (x)}.sup.rcc.sub.k to {circumflex over (x)}.sub.k; ${}_{k|k-1}=\frac{z_k}{z_{k-1}}.Math.{|}_{\hat{z}}=\frac{z_k}{x_k}.Math.\frac{x_k}{x_{k-1}}.Math.\frac{x_{k-1}}{z_{k-1}}=J_{f_z}\left({\hat{x}}_k\right).Math.{\mathrm{AJ}}_{\mathrm{fx}}\left({\hat{z}}_{k-1}\right);\mathrm{and}$ $Q_{k-1}^{\mathrm{MSC}}=J_{\mathrm{fz}}\left({\hat{x}}_{k|x}\right).Math.Q_{k-1}.Math.{J_{\mathrm{fz}}\left({\hat{x}}_{k|x}\right)}^{}.$

10. The angle only (AO) target tracking and estimation method according to claim 1, wherein a state vector in reference Cartesian coordinate (RCC) is defined as a 91 vector as follows: $x^{\mathrm{rcc}}=x=\left[{\stackrel{.fwdarw.}{r}}_{\mathrm{ts}}|{\stackrel{.}{\stackrel{.fwdarw.}{r}}}_{\mathrm{ts}}\right]=\left[x_i\right],i=1,2,\mathrm{.Math.}.Math..Math.9$ $\mathrm{where}.Math..Math.{\stackrel{.}{\stackrel{.fwdarw.}{r}}}_{\mathrm{ts}}=x_4.Math.i+x_5.Math.j+x_6.Math.k=3.Math.\text{-}.Math.D$ velocity vector of {right arrow over (r)}.sub.ts.

11. The angle only (AO) target tracking and estimation method according to claim 1, wherein a state vector in modified spherical coordinate (MSC) is defined as a 91 vector as follows: $z^{\mathrm{msc}}={\left[z_1.Math..Math.z_2.Math..Math.z_3.Math..Math.\mathrm{.Math.}.Math..Math.z_9\right]}^{}=[\frac{1}{r}.Math..Math..Math..Math..Math..Math.\frac{\stackrel{.}{r}}{r}.Math..Math..Math.{.Math.\stackrel{.}{}]}^{}.Math..Math.\mathrm{where}.Math..Math.r=.Math.{\stackrel{.fwdarw.}{r}}_{\mathrm{ts}}.Math.=\mathrm{range}=\sqrt{x_1^2+x_2^2+x_3^2}.Math..Math.\mathrm{and}.Math..Math.=\stackrel{.}{}.Math..Math.\cos .Math..Math..$

12. An angle only (AO) target tracking and estimation method comprising: initializing target states of a modified spherical coordinate (MSC) and reference Cartesian coordinate (RCC) system based on operating conditions of an engagement mission, the engagement mission including a plurality of projectiles and a plurality of targets; calculating modified spherical coordinate (MSC) measurement predictions, including 2 as a function of reference Cartesian coordinate (RCC) and {circumflex over (x)} via a nonlinear mapping function f.sub.z({circumflex over (x)}); and calculating mixed coordinate system blocks, including J.sub.fx, J.sub.fz, , and Q.sup.MSC , to provide for individual mixed AO target state estimator (TSE) processing steps for use in angle only (AO) target tracking and estimation; wherein measurement updating in modified spherical coordinate (MSC) system uses a nine state matrix accounting for position, velocity, and acceleration each in 3 axes to address target maneuvering uncertainty.

13. The angle only (AO) target tracking and estimation method according to claim 12, where measurement updating follows: .sub.k=y.sub.kC{circumflex over (z)}.sub.k|k1, where C=[0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0] is derivative free, {circumflex over (z)}.sub.k|k={circumflex over (z)}.sub.k|k1+K.sub.k, {circumflex over (P)}.sub.k|k={circumflex over (P)}.sub.k|k1KC{circumflex over (P)}.sub.k|k1, K={circumflex over (P)}.sub.k|k1C.sup.TS.sup.1, and S=C{circumflex over (P)}.sub.k|k1C.sup.T+R.sub.k, where R.sub.k is the sensor measurements noise covariance matrix.

14. The angle only (AO) target tracking and estimation method according to claim 12, wherein a steady state 3-D position error in three axes is less than 1 meter.

15. The angle only (AO) target tracking and estimation method according to claim 12, wherein the method is performed on-board a projectile using a single passive sensor.

16. The angle only (AO) target tracking and estimation method according to claim 15, wherein the single passive sensor is configured to track multiple targets.

17. The angle only (AO) target tracking and estimation method according to claim 16, wherein the multiple targets are in motion.

18. A computer program product including one or more non-transitory machine-readable storage mediums having instructions encoded thereon for performing tracking of at least one target, the method comprising: updating angle only (AO) measurements in a modified spherical coordinate (MSC) system from a single passive sensor, wherein measurement updating uses a nine state matrix accounting for position, velocity, and acceleration each in 3 axes to address target maneuvering uncertainty; transforming data from the modified spherical coordinate (MSC) system to a reference Cartesian coordinate (RCC) system; time updating in the reference Cartesian coordinate (RCC) system; transforming data from the reference Cartesian coordinate (RCC) system to the modified spherical coordinate (MSC) system; and calculating the angle only (AO) measurements for a plurality of targets at a sensor interface level for use in guiding a projectile to each of the plurality of targets.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0022] The foregoing and other objects, features, and advantages of the disclosure will be apparent from the following description of particular embodiments of the disclosure, as illustrated in the accompanying drawings in which like reference characters refer to the same parts throughout the different views. The drawings are not necessarily to scale, emphasis instead being placed upon illustrating the principles of the disclosure.

[0023] FIG. 1 shows a diagram of the definitions for a reference Cartesian coordinate (RCC) and a modified spherical coordinate (MSC) formulation according to one embodiment of the present disclosure.

[0024] FIG. 2 is a diagram of one embodiment of the mixed coordinates processing architecture for the system of the present disclosure.

[0025] FIG. 3 illustrates one embodiment of a closed-loop processing scheme of the mixed coordinate system approach of the present disclosure with detailed math operations per the individual steps.

[0026] FIG. 4 illustrates position estimation accuracy and error using a nine state angle only target state estimator in the X, Y, and Z axes according to the principles of the present disclosure.

[0027] FIG. 5 illustrates velocity estimation accuracy and error using a nine state angle only target state estimator in the X, Y, and Z axes according to the principles of the present disclosure.

[0028] FIG. 6 illustrates acceleration estimation accuracy and error using a nine state angle only target state estimator in the X, Y, and Z axes according to the principles of the present disclosure.

[0029] FIG. 7 shows a plot of eight potential targets, with six of the targets flagged to be hit, where multiple measurements of multiple targets are processed by one embodiment of the present disclosure.

DETAILED DESCRIPTION OF THE DISCLOSURE

[0030] Kalman filtering, also known as linear quadratic estimation (LQE), is an algorithm that uses a series of measurements observed over time, containing statistical noise and other inaccuracies, and produces estimates of unknown variables that tend to be more accurate than those based on a single measurement alone, by estimating a joint probability distribution over the variables for each timeframe. As a general notion, prior knowledge of a state is used at a subsequent time step to predict that step (based on a physical model, for example). The step is updated with additional measurement data and there is an output estimate of the current state. This is an iterative process. Essentially, the algorithm works in a two-step process. In the prediction step, the Kalman filter produces estimates of the current state variables, along with their uncertainties. Once the outcome of the next measurement (necessarily corrupted with some amount of error, including random noise) is observed, these estimates are updated using a weighted average, with more weight being given to estimates with higher certainty. The algorithm is recursive. It can run in real time, using only the present input measurements and the previously calculated state and its uncertainty matrix; no additional past information is required.

[0031] The extended Kalman filter (EKF) is a nonlinear version of the Kalman filter which linearizes about an estimate of the current mean and covariance for a state. In the case of well-defined transition models, EKF is considered the de facto standard in the theory of nonlinear state estimation, such as for navigation systems and GPS. In the extended Kalman filter, the state transition and observation models don't need to be linear functions of the state but may instead be differentiable functions.

x.sub.k=f(x.sub.k1, u.sub.k)+w.sub.k

z.sub.k=h(x.sub.k)+v.sub.k

[0032] Here w.sub.k and v.sub.k are the process and observation noises which are both assumed to be zero mean multivariate Gaussian noises with covariance Q.sub.k and R.sub.k respectively. u.sub.k is the control vector.

[0033] The function f can be used to compute the predicted state from the previous estimate and similarly the function h can be used to compute the predicted measurement from the predicted state. However, f and h cannot be applied to the covariance directly. Instead a matrix of partial derivatives (the Jacobian) is computed. At each time step, the Jacobian is evaluated with current predicted states. These matrices can be used in the Kalman filter equations. This process essentially linearizes the non-linear function around the current estimate.

[0034] It is understood that tracking a maneuvering target using angle only measurements provided by an infrared (IR) camera is a challenging design problem. This is especially true for passive IR cameras used as a targeting sensor since its output measurements lack dynamic range information. As such, current target state estimator (TSE) solutions exhibit poor position estimation accuracy (e.g., about a 30 m position estimation error, or larger) depending, in part, on the engagement geometry.

[0035] The present disclosure provides a framework for using a mixed coordinate system (i.e., a combination of Modified Spherical Coordinate (MSC) and Reference Cartesian Coordinate System (RCC)) that is able to estimate the range information (i.e., not physically available from the IR sensor) in a recursive manner. This provides for the ability to more accurately address uncertainty in the position of a maneuvering target by extending a six state TSE (position and velocity each in three axes) into a nine state (position, velocity, and acceleration each in three axes) TSE.

[0036] In one embodiment of the system of the present disclosure, range is estimated via the MSC and recursively used as dynamic information to compute equivalent 3-D state vector information. In certain embodiments of the system of the present disclosure, a full mixed coordinate system (with dynamic range information calculation and exact Jacobian calculation between two coordinate systems (i.e., the MSC and the RCC) preserve the state information allowing the TSE to operate to produce increased positional accuracy.

[0037] The previous six state angle only (AO) TSE, using the Extended Kalman Filter (EKF), was not able to provide a needed accurate TSE vector for guidance subsystems to compute a commanded acceleration for a successful interception with a potential target. The prior six state TSE performance results for a target maneuvering case motivates the need to develop a new angle only filtering design to maintain the TSE estimation accuracy subject to target maneuvering uncertainty. For example, position estimation accuracy for six state model was degraded to an unacceptable error basket (>500 m after 1 second of the fly-out) and velocity estimation accuracy for the six state model was growing much larger than 3 m/s due to unaccountability of the target maneuvering situation.

[0038] The use of a mixed filter design provides several benefits. In one embodiment the system provides a real time (built-in) range estimation as part of the IR measurements, and reduces the sensitivity of the angle only tracking problem to help the TSE solution to stay within a reasonable estimation accuracy (e.g., <2 m vs 30 m or larger for conventional systems). Some benefits of the proposed approach include 1) improving the miss distance; 2) keeping the hardware cost down (without demanding range information from a laser range finder, or the like); and 3) the ability to maintain a TSE estimation performance accuracy to support a guidance subsystem to guide a weapon for a successful interception. In some cases, the success measured by a circular error probable (CEP).

[0039] Referring to FIG. 1, a diagram of the definitions for the RCC and the MSC formulations according to one embodiment of the present disclosure is shown. More specifically, the state vector in RCC is defined as a 91 vector as follows:

[00005]$x^{\mathrm{rcc}}=x=\left[{\stackrel{.fwdarw.}{r}}_{\mathrm{ts}}|{\stackrel{.}{\stackrel{.fwdarw.}{r}}}_{\mathrm{ts}}\right]=\left[x_i\right],i=1,2,\mathrm{.Math.}.Math..Math.9$$\mathrm{where}.Math..Math.{\stackrel{.}{\stackrel{.fwdarw.}{r}}}_{\mathrm{ts}}=x_4.Math.i+x_5.Math.j+x_6.Math.k=3.Math.\text{-}.Math.D$

velocity vector of {right arrow over (r)}.sub.ts. The state vector in MSC is defined as a 91 vector as follows:

[00006]$z^{\mathrm{msc}}={\left[z_1.Math..Math.z_2.Math..Math.z_3.Math..Math.\mathrm{.Math.}.Math..Math.z_9\right]}^{}=[\frac{1}{r}.Math..Math..Math..Math..Math..Math.\frac{\stackrel{.}{r}}{r}.Math..Math..Math.{.Math.\stackrel{.}{}]}^{}$

where r={right arrow over (r)}.sub.ts=range ={square root over (x.sub.1.sup.2+x.sub.2.sup.2+x.sub.3.sup.2)} ={dot over ()}cos. The EO/IR sensor 4 measures and used to calculate the location of the target 2 using the TSE of the present disclosure where the range information is provided via the mixed coordinate system approach described herein.

[0040] Referring to FIG. 2, a diagram of one embodiment of the mixed coordinates processing architecture of the present disclosure is shown. More specifically, the information in MSC is exactly transformed to RCC according to {circumflex over (x)}.sub.k1|k1=f.sub.x({circumflex over (z)}.sub.k1|k1)10.

[0041] In certain embodiments, the nonlinear function mapping of the MSC state to the RCC state, f.sub.x(z) is as follows:

[00007]$.Math.X_1=r.Math..Math.\cos .Math..Math..Math..Math.\cos .Math..Math.=\left(\frac{1}{z_1}\right).Math.\cos \left(z_3\right).Math.\cos \left(z_2\right)$$.Math.x_2=r.Math..Math.\cos .Math..Math..Math..Math.\sin .Math..Math.=\left(\frac{1}{z_1}\right).Math.\cos \left(z_3\right).Math.\sin \left(z_2\right)$$.Math.x_3=r.Math..Math.\sin .Math..Math.=\left(\frac{1}{z_1}\right).Math.\sin \left(z_3\right)$$x_4=\stackrel{.}{r}.Math..Math.\cos .Math..Math..Math..Math.\cos .Math..Math.-r.Math..Math.\stackrel{.}{}.Math..Math.\sin .Math..Math..Math..Math.\cos .Math..Math.-r.Math..Math..Math..Math.\sin .Math..Math.=\left(\frac{1}{z_1}\right)\left[z_4.Math.\cos \left(z_2\right).Math.\cos \left(z_3\right)-z_5.Math.\sin \left(z_2\right)-z_6.Math.\cos \left(z_2\right).Math.\sin \left(z_3\right)\right]$$x_5=\stackrel{.}{r}.Math..Math.\cos .Math..Math..Math..Math.\sin .Math..Math.-r.Math..Math.\stackrel{.}{}.Math..Math.\sin .Math..Math..Math..Math.\sin .Math..Math.+r.Math..Math..Math..Math.\cos .Math..Math.=\left(\frac{1}{z_1}\right)\left[z_4.Math.\cos \left(z_2\right).Math.\cos \left(z_3\right)-z_5.Math.\sin \left(z_2\right)-z_6.Math.\cos \left(z_2\right).Math.\sin \left(z_3\right)\right]$$.Math.x_6=\stackrel{.}{r}.Math..Math.\sin .Math..Math..Math.+r.Math..Math.\stackrel{.}{}.Math..Math.\cos .Math..Math.=\left(\frac{1}{z_1}\right)\left[z_4.Math.\sin \left(z_3\right)+z_6.Math.\cos \left(z_3\right)\right]$$x_7=\left(z_4^{*}.Math.{\sin \left(z_2\right)}^{*}.Math.z_5+{\cos \left(z_2\right)}^{*}.Math.\sin .Math.{\left\{z^3\right)}^{*}.Math.z_6-{\cos \left(z_2\right)}^{*}.Math.{\cos \left(z_3\right)}^{*}.Math.z_4\right)/z_1^2-\left({\sin \left(z_2\right)}^{*}.Math.z_8+{\cos \left(z_2\right)}^{*}.Math.z_5^{*}.Math.z_5-{\cos \left(z_2\right)}^{*}.Math.{\cos \left(z_3\right)}^{*}.Math.z_7^{*}+{\cos \left(z_2\right)}^{*}.Math.{\sin \left(z_3\right)}^{*}.Math.z_9+{\cos \left(z_2\right)}^{*}.Math.{\cos \left(z_3\right)}^{*}.Math.z_6^{*}.Math.z_6+{\cos \left(z_3\right)}^{*}.Math.{\sin \left(z_2\right)}^{*}.Math.z_4^{*}.Math.z_5+\cos .Math.{\left\{z_2\right)}^{*}.Math.{\sin \left(z_3\right)}^{*}.Math.z_4^{*}.Math.z_6-{\sin \left(z_2\right)}^{*}.Math.{\sin \left(z_3\right)}^{*}.Math.z_6^{*}.Math.z_5\right)/z_1$$x_8=-({\sin \left(z_2\right)}^{*}.Math.z_5^{*}.Math.z_5-{\cos \left(z_2\right)}^{*}.Math.z_8-{\cos \left(z_3\right)}^{*}.Math.{\sin \left(z_2\right)}^{*}.Math.z_7+{\sin \left(z_2\right)}^{*}.Math.{\sin \left(z_3\right)}^{*}.Math.z_9-{\cos \left(z_2\right)}^{*}.Math.{\cos \left(z_3\right)}^{*}.Math.z_4^{*}.Math.z_5+{\cos \left(z_2\right)}^{*}.Math.{\sin \left(z_3\right)}^{*}.Math.z_6^{*}.Math.z_5+\cos .Math.{(}_{z.Math..Math.3)}^{*}.Math.{\sin \left(z_2\right)}^{*}.Math.z_6^{*}.Math.z_6+{\sin \left(z_2\right)}^{*}.Math.{\sin \left(z_3\right)}^{*}.Math.z_4^{*}.Math.z_6)/z_1-\left(z_4^{*}.Math.{\cos \left(z_2\right)}^{*}.Math.z_5+{\cos \left(z_3\right)}^{*}.Math.{\sin \left(z_2\right)}^{*}.Math.z_4-{\sin \left(z_2\right)}^{*}.Math.{\sin \left(z_3\right)}^{*}.Math.z_6\right)/z_1^2.Math..Math.x_9=\left({\cos \left(z_3\right)}^{*}.Math.z_6+{\sin \left(z_3\right)}^{*}.Math.z_4^{*}.Math.z_4\right)/z_1^2-\left({\cos \left(z_3\right)}^{*}.Math.z_9+{\sin \left(z_3\right)}^{*}.Math.z_7+{\cos \left(z_3\right)}^{*}.Math.z_4^{*}.Math.z_6-{\sin \left(z_3\right)}^{*}.Math.z_6^{*}.Math.z_6\right)/z_1.$

[0042] The time update step 12 is in RCC according to {circumflex over (x)}.sub.k|k1=A{circumflex over (x)}.sub.k1|k1+B.sub.ww.sub.k+B.sub.uu.sub.k where B.sub.u=B.sub.w and A=a nine state EKF TSE

[00008]$\left[\begin{array}{c}x\left(n+1\right)\\ y\left(n+1\right)\\ z\left(n+1\right)\\ v_x\left(n+1\right)\\ v_y\left(n+1\right)\\ v_z\left(n+1\right)\\ a_x\left(n+1\right)\\ a_y\left(n+1\right)\\ a_z\left(n+1\right)\end{array}\right]=\left[\begin{array}{ccccccccc}1& 0& 0& t& 0& 0& 0.5.Math.t^2& 0& 0\\ 0& 1& 0& 0& t& 0& 0& 0.5.Math.t^2& 0\\ 0& 0& 1& 0& 0& t& 0& 0& 0.5.Math.t^2\\ 0& 0& 0& 1& 0& 0& t& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& t& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& t\\ 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right].Math.\left[\begin{array}{c}x\left(n\right)\\ y\left(n\right)\\ z\left(n\right)\\ v_x\left(n\right)\\ v_y\left(n\right)\\ v_z\left(n\right)\\ a_x\left(n\right)\\ a_y\left(n\right)\\ a_z\left(n\right)\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ \frac{t}{m}& 0& 0\\ 0& \frac{t}{m}& 0\\ 0& 0& \frac{t}{m}\end{array}\right]\left[\begin{array}{c}{F}_x\\ F_y\\ F_z\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ \frac{t}{m}& 0& 0\\ 0& \frac{t}{m}& 0\\ 0& 0& \frac{t}{m}\end{array}\right]\left[\begin{array}{c}{F}_{\mathrm{xd}}\\ F_{y.Math..Math.d}\\ F_{\mathrm{zd}}\end{array}\right]$

[0043] {circumflex over (P)}.sub.k|k1=.sub.k|k1{circumflex over (P)}.sub.k1|k1.sub.k|k1+Q.sub.k1.sup.msc, where Q.sub.k1 the rcc state vector is renamed, {circumflex over (x)}.sup.rcc.sub.k to {circumflex over (x)}.sub.k;

[00009]${}_{k|k-1}=\frac{z_k}{z_{k-1}}.Math.{|}_{\hat{z}}=\frac{z_k}{x_k}.Math.\frac{x_k}{x_{k-1}}.Math.\frac{x_{k-1}}{z_{k-1}}=J_{f_z}\left({\hat{x}}_k\right).Math.{\mathrm{AJ}}_{\mathrm{fx}}\left({\hat{z}}_{k-1}\right);$$Q_{k-1}^{\mathrm{MSC}}=J_{\mathrm{fz}}\left({\hat{x}}_{k|x}\right).Math.Q_{k-1}.Math.{J_{\mathrm{fz}}\left({\hat{x}}_{k|x}\right)}^{}.$

Here, the J terms represent the Jacobian matrix of MSC and RCC states.

[0044] The Jacobian Jz is computed as follows where z.sub.i and x.sub.i, i=1, 2, . . . 9 (MSC and RCC state variables) are defined above.

[00010]$J_x\left(z_1,z_2,z_3,\mathrm{.Math.}.Math..Math.z_9\right)=\left[\begin{array}{ccccc}{J}_{x.Math..Math.11}& J_{x.Math..Math.12}& \mathrm{.Math.}& \mathrm{.Math.}& J_{x.Math..Math.19}\\ J_{x.Math..Math.21}& J_{x.Math..Math.22}& \mathrm{.Math.}& \mathrm{.Math.}& J_{x.Math..Math.29}\\ J_{x.Math..Math.31}& J_{x.Math..Math.32}& \mathrm{.Math.}& \mathrm{.Math.}& J_{x.Math..Math.39}\\ J_{x.Math..Math.41}& J_{x.Math..Math.42}& \mathrm{.Math.}& \mathrm{.Math.}& J_{x.Math..Math.49}\\ & \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}& \\ & \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}& \\ J_{x.Math..Math.91}& J_{x.Math..Math.92}& \mathrm{.Math.}& \mathrm{.Math.}& J_{x.Math..Math.99}\end{array}\right]$$\mathrm{where}.Math..Math.J_{\mathrm{xij}}=\frac{x_i}{z_j},i=1.Math..Math.\mathrm{to}.Math..Math.9,.Math.j=1.Math..Math.\mathrm{to}.Math..Math.9.$$J_z\left(x_1,x_2,x_3,\mathrm{.Math.}.Math..Math.x_9\right)=\left[\begin{array}{ccccc}{J}_{z.Math..Math.11}& J_{z.Math..Math.12}& \mathrm{.Math.}& \mathrm{.Math.}& J_{z.Math..Math.19}\\ J_{z.Math..Math.21}& J_{z.Math..Math.22}& \mathrm{.Math.}& \mathrm{.Math.}& J_{z.Math..Math.29}\\ J_{z.Math..Math.31}& J_{z.Math..Math.32}& \mathrm{.Math.}& \mathrm{.Math.}& J_{z.Math..Math.39}\\ J_{z.Math..Math.41}& J_{z.Math..Math.42}& \mathrm{.Math.}& \mathrm{.Math.}& J_{z.Math..Math.49}\\ & \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}& \\ & \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}& \\ J_{z.Math..Math.91}& J_{z.Math..Math.92}& \mathrm{.Math.}& \mathrm{.Math.}& J_{z.Math..Math.99}\end{array}\right]$$\mathrm{where}.Math..Math.J_{\mathrm{zij}}=\frac{z_i}{x_j},i=1.Math..Math.\mathrm{to}.Math..Math.9,.Math.j=1.Math..Math.\mathrm{to}.Math..Math.9.$

[0045] Still referring to FIG. 2, following the time update a conversion from RCC to MSC 14 is completed according to {circumflex over (z)}.sub.k|k1=f.sub.z({circumflex over (x)}.sub.k|k1).

[0046] In certain embodiments, the nonlinear function mapping of the RCC state to the MSC state, f.sub.z(x) is as follows:

[00011]$Z_1=\frac{1}{r}=\frac{1}{\sqrt{x_1^2+x_2^2+x_3^2}}$$Z_2==a.Math..Math.\tan \left(\frac{x_2}{x_1}\right)$$z_3==a.Math..Math.\tan \left(\frac{x_3}{\sqrt{x_1^2+x_2^2}}\right)$$Z_4=\frac{\stackrel{.}{r}}{r}=\frac{x_1.Math.x_4+x_2.Math.x_5+x_3.Math.x_6}{x_1^2+x_2^2+x_3^2}$$Z_5==.Math.\stackrel{.}{}.Math..Math.\cos .Math..Math.=\frac{x_1.Math.x_5-x_2.Math.x_4}{x_1^2+x_2^2}.Math.\cos (a.Math..Math.\tan \left(\frac{x_3}{\sqrt{x_1^2+x_2^2}}\right).Math..Math.Z_6=\stackrel{.}{}=\frac{x_6\left(x_1^2+x_2^2\right)-x_3\left(x_1.Math.x_4+x_2.Math.x_5\right)}{\left(x_1^2+x_2^2+x_3^2\right).Math.\sqrt{x_1^2+x_2^2}}.Math..Math.z_7=\frac{d\left(\stackrel{.}{r}\right)}{\mathrm{dt}\left(r\right)}=\frac{\stackrel{\mathrm{.Math.}}{r}.Math..Math.r-\stackrel{.}{r}.Math..Math.\stackrel{.}{r}}{r^2}=\frac{\stackrel{\mathrm{.Math.}}{r}}{r}-\frac{{\stackrel{.}{r}}^2}{r^2}.Math..Math.Z_8=\stackrel{.}{}=\stackrel{\mathrm{.Math.}}{}.Math..Math.\cos .Math..Math.-\stackrel{.}{}.Math..Math.\sin .Math..Math..Math..Math.Z_9=\stackrel{\mathrm{.Math.}}{}$

[0047] This data is fed into the measurement update phase of the process 16, where the measurements update is in MSC according to the following:

[0048] .sub.k=y.sub.kC{circumflex over (z)}.sub.k|k1, where C=[0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0] is derivative free.

{circumflex over (z)}.sub.k|k={circumflex over (z)}|k1K.sub.k

{circumflex over (P)}.sub.k|k{circumflex over (P)}.sub.k|k1KC{circumflex over (P)}.sub.k|k1

K={circumflex over (P)}.sub.k|k1C.sup.TS.sup.1

[0049] S=C{circumflex over (P)}.sub.k|k1C.sup.T+R.sub.k, where R.sub.k is the IR measurements noise covariance matrix. The cycle repeats itself by proceeding to the MSC to RCC transformation phase 10.

[0050] Referring to FIG. 3, one embodiment of a closed-loop processing scheme of the mixed coordinate system approach of the present disclosure with detailed math operations per the individual steps is shown.

[0051] In one embodiment of the system of the present disclosure, the main function for the nine state EKF are as follows:

[0052] function [X_k, P_k]=TSE_EKF_v2_9_state(X_k, P_k, y_k, Q, R, dT)

[0053] % Inputs

[0054] % X_k=previous state estimate in Cartesian coordinates (n1)

[0055] % P_k=previous covariance matrix (nn)

[0056] % y_k=observation from EOIR sensor z1: az z2: e1 (m1)

[0057] % Q=process noise covariance matrix (nn)

[0058] % R=observation error matrix (mm)

[0059] % dT=sensor propagation time step

[0060] % Outputs

[0061] % X_k=current state estimate in Cartesian coordinates (n1)

[0062] % P_k=current covariance matrix (nn)

[0063] %%

[0064] X_prev=X_k;

[0065] Z_prev=f_z(X_prev);

[0066] %% 1) State Estimate Prediction (Time Update)

[0067] A=[eye(3), dT*eye(3), *dT2*eye(3);

[0068] zeros(3), eye(3), dT*eye(3);

[0069] zeros(3,6), eye(3)];

[0070] B=[zeros(6,3); dT*eye(3)];

[0071] X_k=A*X_prev; % X_k from A*X_k1

[0072] Z_k=f_z(X_k); % Z_k from X_k

[0073] Phi=Jfz(X_k)*A*Jfx(Z_prev);

[0074] Q_mcs =Jfz(X_k)*Q*Jfz(X_k);

[0075] P_k=Phi*P_k*Phi+Q_mcs;

[0076] %% 2) Observation Residual

[0077] C=zeros (2,9);

[0078] C(1,2)=1;

[0079] C (2,3)=1;

[0080] e=y_kC*Z_k;

[0081] %% 3) Gain Matrix

[0082] S=C*P_k*C+R; % (mm)

[0083] K=P_k*C*S(1); % (nm)

[0084] %% 4) Update State Estimate Prediction

[0085] Z_k=Z_k+K*e; % update predicted state estimate (n1)

[0086] X_k=f x(Z_k);

[0087] P_k=P_kK*C*P_k; % update predicted covariance matrix (nn)

[0088] end.

[0089] Fx formation: function X=f_x(Z)

[0090] z1=Z(1); z2=Z(2); z3=Z(3); z4=Z(4); z5=Z(5); z6=Z(6); z7=Z(7); z8=Z(8); z9=Z(9); X=zeros (9,1); X(1)=(cos(z2)*cos(z3))/z1; X(2)=(cos(z3)*sin(z2))/z1; X(3)=sin(z3)/z1; X(4)=(z4*cos(z2)*cos(z3)+z1*z5*cos(z3)*sin(z2)+z1*z6*cos(z2)*sin(z3))/z12; X(5)=(z4*cos(z3)*sin(z2)z1*z5*cos(z2)*cos(z3)+z1*z6*sin(z2)*sin(z3))/z12; X(6)=(z4*cos(z3)*sin(z2)z1*z5*cos(z2)*cos(z3)+z1*z6*sin(z2)*sin(z3))/z12; X(7)=(z1*z7*cos(z2)*cos(z3)2*z42*cos(z2)*cos(z3)+z12*z8*cos(z3)*sin(z2)+z12*z9*cos(z2)*sin(z3)+z12*z52*cos(z2)*cos(z3)+z12*z62*cos(z2)*cos(z3)2*z12*z5*z6*sin(z2)*sin(z3)2*z1*z4*z5*cos(z3)*sin(z2)2*z1*z4*z6*cos(z2)*sin(z3))/z13; X(8)=(z1*z7*cos(z3)*sin(z2)2*z42*cos(z3)*sin(z2)z12*z8*cos(z2)*cos(z3)+z12*z9*sin(z2)*sin(z3)+z12*z52*cos(z3)*sin(z2)+z12*z62*cos(z3)*sin(z2)+2*z1*z4*z5*cos(z2)*cos(z3) 2*z1*z4*z6*sin(z2)*sin(z3)+2*z12*z5*z6*cos(z2)*sin(z3))/z13; X(9)=(z1*z7*sin(z3)2*z42*sin(z3)z12*z9*cos(z3)+z12*z62*sin(z3)+2*z1*z4*z6*cos(z3))/z13; end.

[0091] Jacobian Fz: %% Jacobian of F_z(X) function Jfz=Jfz(X)

[0092] x1=X(1); x2=X(2); x3=X(3); x4=X(4); x5=X(5); x6=X(6); x7=X(7); x8=X(8); x9=X(9);

[0093] Jfz=zeros (9,9); Jfz(1,1)=x1/(x12+x22+x32)(3/2); Jfz(1,2)=x2/(x12+x22+x32)(3/2); Jfz(1,3)=x3/(x12+x22+x32)(3/2);

[0094] Jfz(2,1)=x2/(x12*(x22/x12+1)); Jfz(2,2)=1/(x1*(x22/x12+1));

[0095] Jfz(3,1)=(x1*x3)/((x32/(x12+x22)+1)*(x12+x22)(3/2)); Jfz(3,2)=(x2*x3)/((x32/(x12+x22)+1)*(x12+x22)(3/2)); Jfz(3,3)=1/((x32/(x12+x22)+1)*(x12+x22)(1/2));

[0096] Jfz(4,1)=(3*x1*(x1*x4+x2*x5+x3*x6))/(x12+x22+x32)(5/2)x4/(x12+x22+x32)(3/2); Jfz(4,2)=(3*x2*(x1*x4+x2*x5+x3*x6))/(x12+x22+x32)(5/2)x5/(x12+x22+x32)(3/2); Jfz(4,3)=(3*x3*(x1*x4+x2*x5+x3*x6))/(x12+x22+x32)(5/2)x6/(x12+x22+x32)(3/2); Jfz(4,4)=x1/(x12+x22+x32)(3/2); Jfz(4,5)=x2/(x12+x22+x32)(3/2); Jfz(4,6)=x3/(x12+x22+x32)(3/2);

[0097] Jfz(5, 1)=x5/(x12+x22)(2*x1*(x1*x5x2*x4))/(x12+x22)2; Jfz(5,2)=x4/(x12+x22)(2*x2*(x1*x5x2*x4))/(x12+x22)2; Jfz(5,4)=x2/(x12+x22); Jfz(5,5)=x1/(x12+x22);

[0098] Jfz(6,1)=(2*x1*x32*(x6/(x12+x22)(1/2)(x3*(x1*x4+x2*x5))/(x12+x22)(3/2)))/((x32/(x12+x22)+1)2*(x12+x22)2)((x1*x6)/(x12+x22)(3/2)+(x3*x4)/(x12+x22)(3/2)(3*x1*x3*(x1*x4+x2*x5))/(x12+x22)(5/2))/(x32/(x12+x22)+1); Jfz(6,2)=(2*x2*x32*(x6/(x12+x22)(1/2)(x3*(x1*x4+x2*x5))/(x12+x22)(3/2)))/((x32/(x12+x22)+1)2*(x12+x22)2)((x2*x6)/(x12+x22)(3/2)+(x3*x5)/(x12+x22)(3/2)(3*x2*x3*(x1*x4+x2*x5))/(x12+x22)(5/2))/(x32/(x12+x22)+1); Jfz(6,3)=(x1*x4+x2*x5)/((x32/(x12+x22)+1)*(x12+x22)(3/2))(2*x3*(x6/(x12+x22)(1/2)(x3*(x1*x4+x2*x5))/(x12+x22)(3/2)))/((x32/(x12+x22)+1)2*(x12+x22)); Jfz(6,4)=(x1*x3)/((x32/(x12+x22)+1)*(x12+x22)(3/2)); Jfz(6,5)=(x2*x3)/((x32/(x12+x22)+1)*(x12+x22)(3/2)); Jfz(6,6)=1/((x32/(x12+x22)+1)*(x12+x22)(1/2));

[0099] Jfz(7, 1)=(6*x4*(x1*x4+x2*x5+x3*x6))/(x12+x22+x32)(5/2)x7/(x12+x22+x32)(3/2)(15*x1*(x1*x4+x2*x5+x3*x6)2)/(x12+x22+x32)(7/2)+(3*x1*(x1*x7+x2*x8+x3*x9+x42+x52+x62))/(x12+x22+x32)(5/2); Jfz(7,2)=(6*x5*(x1*x4+x2*x5+x3*x6))/(x12+x22+x32)(5/2)x8/(x12+x22+x32)(3/2)(15*x2*(x1*x4+x2*x5+x3*x6)2)/(x12+x22+x32)(7/2)+(3*x2*(x1*x7+x2*x8+x3*x9+x42+x52+x62))/(x12+x22+x32)(5/2); Jfz(7,3)=(6*x6*(x1*x4 30 x2*x5+x3*x6))/(x12+x22+x32)(5/2)x9/(x12+x22+x32)(3/2)(15*x3*(x1*x4+x2*x5+x3*x6)2)/(x12+x22+x32)(7/2)+(3*x3*(x1*x7+x2*x8+x3*x9+x42+x52+x62))/(x12+x22+x32)(5/2); Jfz(7,4)=(6*x1*(x1*x4+x2*x5+x3*x6))/(x12+x22+x32)(5/2)(2*x4)/(x12+x22+x32)(3/2); Jfz(7,5)=(6*x2*(x1*x4+x2*x5+x3*x6))/(x12+x22+x32)(5/2)(2*x5)/(x12+x22+x32)(3/2); Jfz(7,6)=(6*x3*(x1 *x4+x2*x5+x3*x6))/(x12+x22+x32)(5/2)(2*x6)/(x12+x22+x32)(3/2); Jfz(7,7)=x1/(x12+x22+x32)(3/2); Jfz(7,8)=x2/(x12+x22+x32)(3/2); Jfz(7,9)=x3/(x12+x22+x32)(3/2);

[0100] Jfz(8, 1)=(2*x2*x422*x2*x52+3*x12*x8+x22*x82*x1*x2*x74*x1*x4*x5)/(x12+x22)2(4*x1*(x13*x8x23*x7+2*x1*x2*x422*x1 *x2*x52x12*x2*x72*x12*x4*x5+x1 *x22*x8+2*x22*x4*x5))/(x12+x22)3; Jfz(8,2)=(2*x1*x422*x1*x52x12*x73*x22*x7+2*x1 *x2*x8+4*x2*x4*x5)/(x12+x22)2(4*x2*(x13*x8x23*x7+2*x1*x2*x422*x 1*x2*x52x12*x2*x72*x12*x4*x5+x1*x22*x8+2*x22*x4*x5))/(x12+x22)3; Jfz(8,4)=(2*x22*x52*x12*x5+4*x1*x2*x4)/(x12+x22)2; Jfz(8,5)=(2*x12*x42*x22*x4+4*x1 *x2*x5)/(x12+x22)2; Jfz(8,7)=(x12*x2+x23)/(x12+x22)2; Jfz(8,8)=(x1*x22+x13)/(x12+x22)2;

[0101] Jfz(9, 1)=((x6/(x12+x22)(1/2)(x3*(x1*x4+x2*x5))/(x12 +x22)(3/2))*((2*x32*x4)/(x12+x22)2(4*x1*x32*(2*x1 *x4+2*x2*x5))/(x12 +x22)3+(4*x1 *x3*x6)/(x12+x22)2))/(x32/(x12+x22)+1)2(((x32*(2*x1*x4+2*x2*x5))/(x12+x22)2(2*x3*x6)/(x12+x22))*((x1*x6)/(x12+x22)(3/2)+(x3*x4)/(x12+x2)(3/2)(3*x1 *x3*(x1*x4 +x2*x5))/(x12+x22)(5/2)))/(x32/(x12+x22)+1)2((x1*x9)/(x12+x22)(3/2)+(x3*x7)/(x12+x22)(3/2)+(2*x4*x6)/(x12+x22)(3/2)+(15*x1*x3*(x1*x4+x2*x5)2)/(x12+x22)(7/2)(3*x1*x3*(2*x1*x7+2*x2*x8+2*x42+2*x52))/(2*(x12+x22)(5/2))(6*x3*x4*(x1*x4+x2*x5))/(x12+x22)(5/2)(3*x1*x6*(2*x1*x4+2*x2*x5))/(x12+x22)(5/2))/(x32/(x12+x22)+1)+(2*x1*x32*(x9/(x12+x22)(1/2)(x6*(2*x1*x4+2*x2*x5))/(x12+x22)(3/2)+(3*x3*(x1*x4+x2*x5)2)/(x12+x22)(5/2)(x3*(2*x1*x7+2*x2*x8+2*x42+2*x52))/(2*(x12+x22)(3/2))))/((x32/(x12+x22)+1)2*(x12+x22)2)+(4*x1*x32*((x32*(2*x1*x4+2*x2*x5))/(x12+x22)2(2*x3*x6)/(x12+x22))*(x6/(x12+x22)(1/2)(x3*(x1*x4+x2*x5))/(x12+x22)(3/2)))/((x32/(x12+x22)+1)3*(x12+x22)2); Jfz(9,2)=((x6/(x12+x22)(1/2)(x3*( x1*x4+x2*x5))/(x12+x22)(3/2))*((2*x32*x5)/(x12+x22)2(4*x2*x32*(2*x1*x4+2*x2*x5))/(x12+x22)3+(4*x2*x3*x6)/(x12+x22)2))/(x32/(x12+x22)+1)2(((x32*(2*x1*x4+2*x2*x5))/(x12+x22)2(2*x3*x6)/(x12+x22))*((x2*x6)/(x12+x22)(3/2)+(x3*x5)/(x12 +x22)(3/2) (3*x2*x3*(x1*x4+x2*x5))/(x12+x22)(5/2)))/(x32/(x12+x22)+1)2((x2*x9)/(x12+x22)(3/2)+(x3*x8)/(x12+x22)(3/2)+(2*x5*x6)/(x12+x22)(3/2)+(15*x2*x3*(x1*x4+x2*x5)2)/(x12+x22)(7/2)(3*x2*x3*(2*x1*x7+2*x2*x8+2*x42+2*x52))/(2*(x12+x22)(5/2))(6*x3*x5*(x1*x4+x2*x5))/(x12+x22)(5/2)(3*x2*x6*(2*x1*x4+2*x2*x5))/(x12+x22)(5/2))/(x32/(x12+x22)+1)+(2*x2*x32*(x9/(x12+x22)(1/2)(x6*(2*x1*x4+2*x2*x5))/(x12+x22)(3/2)+(3*x3*(x1*x4 +x2*x5)2)/(x12+x22)(5/2)(x3*(2*x1*x7+2*x2*x8+2*x42+2*x52))/(2*(x12+x22)(3/2))))/((x32/(x12+x22)+1)2*(x12+x22)2)+(4*x2*x32*((x32*(2*x1*x4+2*x2*x5))/(x12+x22)2(2*x3*x6)/(x12+x22))*(x6/(x12+x22)(1/2)(x3*(x1*x4x2*x5))/(x12+x22)(3/2)))/((x32/(x12+x22)+1)3*(x12+x22)2); Jfz(9,3)=((3*(x1*x4+x2*x5)2)/(x12+x22)(5/2)(2*x1*x7+2*x2*x8+2*x42+2*x52)/(2*(x12+x22)(3/2)))/(x32/(x12+x22)+1)((x6/(x12+x22)(1/2)(x3*(x1*x4+x2*x5))/(x12+x22)(3/2))*((2*x6)/(x12+x22)(2*x3*(2*x1*x4+2*x2*x5)/(x12+x22)2))/(x32/(x12+x22)+1)2(2*x3*(x9/(x12+x22)(1/2)(x6*(2*x1*x4+2*x2*x5))/(x12+x22)(3/2)+(3*x3*(x1*x4 +x2*x5)2)/(x12+x22)(5/2)(x3*(2*x1*x7+2*x2*x8+2*x42+2*x52))/(2*(x12+x22)(3/2))))/((x32/(x12+x22)+1)2*(x12+x22))(((x32*(2*x1*x4+2*x2*x5))/(x12+x22)2(2*x3*x6)/(x12+x22))*(x1*x4+x2*x5))/((x32/(x12+x22)+1)2*(x12+x22)(3/2))(4*x3*((x32*(2*x1*x4+2*x2*x5))/(x12+x22)2(2*x3*x6)/(x12+x22))*(x6/(x12+x22)(1/2)(x3*(x1*x4+x2*x5))/(x12+x22)(3/2)))/((x32/(x12+x22)+1)3*(x12+x22)); Jfz(9,4)=(2*x1 *x32*(x6/(x12+x22)(1/2)(x3*(x1*x4+x2*x5))/(x12+x22)(3/2)))/((x32/(x12+x22)+1)2*(x12+x22)2)((2*x1*x6)/(x12+x22)(3/2)+(2*x3*x4)/(x12+x22)(3/2)(6*x1*x3*(x1*x4+x2*x5))/(x12+x22)(5/2))/(x32/(x12+x22)+1)(x1*x3*((x32*(2*x1*x4+2*x2*x5))/(x12+x22)2(2*x3*x6)/(x12+x22)))/((x32/(x12+x22)+1)2*(x12+x22)(3/2)); Jfz(9,5)=(2*x2*x32*(x6/(x12+x22)(1/2)(x3*(x1*x4+x2*x5))/(x12+x22)(3/2)))/((x32/(x12+x22)+1)2*(x12+x22)2)((2*x2*x6)/(x12+x22)(3/2)+(2*x3*x5)/(x12+x22)(3/2)(6*x2*x3*(x1*x4+x2*x5))/(x12+x22(5/2))/(x32/(x12+x22)+1)(x2*x3*((x32*(2*x1*x4+2*x2*x5))/(x12+x22)2(2*x3*x6)/(x12+x22)))/((x32/(x12+x22)+1)2*(x12+x22)(3/2)); Jfz(9,6)=((x32*(2*x1*x4+2*x2*x5))/(x12+x22)2(2*x3*x6)/(x12+x22))/((x32/(x12+x22)+1)2*(x12+x22)(1/2))(2*x1*x4+2*x2*x5)/((x32/(x12+x22)+1)*(x12+x22)(3/2))(2*x3*(x6/(x12+x22)(1/2)(x3*(x1*x4+x2*x5))/(x12+x22)(3/2)))/((x32/(x12+x22)+1)2*(x12+x22)); Jfz(9,7)=(x1*x3)/((x32/(x12+x22)+1)*(x12+x22)(3/2)); Jfz(9,8)=(x2*x3)/((x32/(x12+x22)+1)*(x12+x22)(3/2)); Jfz(9,9)=1/((x32/(x12+x22)+1)*(x12+x22)(1/2)); end.

[0102] Jacobian Fx(z): %% Jacobian of F_x(Z) function Jfx=Jfx(Z)

[0103] z1=Z(1); z2=Z(2); z3=Z(3); z4=Z(4); z5=Z(5); z6=Z(6); z7=Z(7); z8=Z(8); z9=Z(9); Jfx=zeros(9,9);

[0104] Jfx(1,1)=(cos(z2)*cos(z3))/z12; Jfx(1,2)=(cos(z3)*sin(z2))/z1; Jfx(1,3)=(cos(z2)*sin(z3))/z1;

[0105] Jfx(2,1)=(cos(z3)*sin(z2))/z12; Jfx(2,2)=(cos(z2)*cos(z3))/z1; Jfx(2,3)=(sin(z2)*sin(z3))/z1;

[0106] Jfx(3,1)=sin(z3)/z12; Jfx(3,3)=cos(z3)/z1;

[0107] Jfx(4,1)=(2*(z4*cos(z2)*cos(z3)+z1*z5*cos(z3)*sin(z2)+z1*z6*cos(z2)*sin(z3)))/z13(z5*cos(z3)*sin(z2)+z6*cos(z2)*sin(z3))/z12; Jfx(4,2)=(z4*cos(z3)*sin(z2)z1*z5*cos(z2)*cos(z3)+z1*z6*sin(z2)*sin(z3))/z12; Jfx(4,3)=(z4*cos(z2)*sin(z3)z1*z6*cos(z2)*cos(z3)+z1*z5*sin(z2)*sin(z3))/z12; Jfx(4,4)=(cos(z2)*cos(z3))/z12; Jfx(4,5)=(cos(z3)*sin(z2))/z1; Jfx(4,6)=(cos(z2)*sin(z3))/z1;

[0108] Jfx(5,1)=(2*(z4*cos(z3)*sin(z2)z1*z5*cos(z2)*cos(z3)+z1*z6*sin(z2)*sin(z3)))/z13+(z5*cos(z2)*cos(z3)z6*sin(z2)*sin(z3))/z12; Jfx(5,2) =(z4*cos(z2)*cos(z3)+z1*z5*cos(z3)*sin(z2)+z1*z6*cos(z2)*sin(z3))/z12; Jfx(5,3) =(z*1z5*cos(z2)*sin(z3)z4*sin(z2)*sin(z3)+z1*z6*cos(z3)*sin(z2))/z12; Jfx(5,4) =(cos(z3)*sin(z2))/z12; Jfx(5,5)=(cos(z2)*cos(z3))/z1; Jfx(5,6)=(sin(z2)*sin(z3))/z1 ;

[0109] Jfx(6,1)=(2*(z4*cos(z3)*sin(z2)z1*z5*cos(z2)*cos(z3)+z1*z6*sin(z2)*sin(z3)))/z13+(z5*cos(z2)*cos(z3)z6*sin(z2)*sin(z3))/z12; Jfx(6,2) =(z4*cos(z2)*cos(z3)+z1*z5*cos(z3)*sin(z2)+z1*z6*cos(z2)*sin(z3))/z12; Jfx(6,3) =(z1*z5*cos(z2)*sin(z3)z4*sin(z2)*sin(z3)+z1*z6*cos(z3)*sin(z2))/z12; Jfx(6,4) =(cos(z3)*sin(z2))/z12; Jfx(6,5)=(cos(z2)*cos(z3))/z1; Jfx(6,6)=(sin(z2)*sin(z3))/z1;

[0110] Jfx(7,1)=(3*(z1*z7*cos(z2)*cos(z3)2*z42*cos(z2)*cos(z3)+z12*z8*cos(z3)*sin(z2)+z12*z9*cos(z2)*sin(z3)+z12*z52*cos(z2)*cos(z3)+z12*z62*cos(z2)*cos(z3)2*z12*z5*z6*sin(z2)*sin(z3)2*z1*z4*z5*cos(z3)*sin(z2)2*z1*z4*z6*cos(z2)*sin(z3)))/z14(z7*cos(z2)*cos(z3) +2*z1*z8*cos(z3)*sin(z2)2*z4*z5*cos(z3)*sin(z2)+2*z1*z9*cos(z2)*sin(z3)2*z4*z6*cos(z2)*sin(z3)+2*z1*z52*cos(z2)*cos(z3)+2*z1*z62*cos(z2)*cos(z3)4*z1*z5*z6*sin(z2)*sin(z3))/z13; Jfx(7,2)=(z*z7*cos(z3)*sin(z2)2*z42*cos(z3)*sin(z2)z12*z8*cos(z2)*cos(z3)+z12*z9*sin(z2)*sin(z3)+z12*z52*cos(z3)*sin(z2)+z12*z62*cos(z3)*sin(z2)+2*z1*z4*z5*cos(z2)*cos(z3) 2*z1*z4*z6*sin(z2)*sin(z3)+2*z12*z5*z6*cos(z2)*sin(z3))/z13; Jfx(7,3)=(z1*z7*cos(z2)*sin(z3)2*z42*cos(z2)*sin(z3)z12*z9*cos(z2)*cos(z3)+z12*z8*sin(z2)*sin(z3)+z12*z52*cos(z2)*sin(z3)+z12*z62*cos(z2)*sin(z3)+2*z1*z4*z6*cos(z2)*cos(z3)2*z1*z4*z5*sin(z2)*sin(z3)+2*z12*z5*z6*cos(z3)*sin(z2))/z13; Jfx(7,4)=(4*z4*cos(z2)*cos(z3)+2*z1*z5*cos(z3)*sin(z2)2*z1*z6*cos(z2)*sin(z3))/z13; Jfx(7,5)=(2*z1*z4*cos(z3)*sin(z2)2*z12*z5*cos(z2)*cos(z3)+2*z12*z6*sin(z2)*sin(z3))/z13; Jfx(7,6)=(2*z1*z4*cos(z2)*sin(z3)2*z12*z6*cos(z2)*cos(z3)+2*z12*z5*sin(z2)*sin(z3))/z13; Jfx(7,7)=(cos(z2)*cos(z3))/z12; Jfx(7,8)=(cos(z3)*sin(z2))/z1; Jfx(7,9)=(cos(z2)*sin(z3))/z1;

[0111] Jfx(8,1)=(3*(z1*z7*cos(z3)*sin(z2)2*z42*cos(z3)*sin(z2)z12*z8*cos(z2)*cos(z3)+z12*z9*sin(z2)*sin(z3)+z12*z52*cos(z3)*sin(z2)+z12*z62*cos(z3)*sin(z2)2*z1*z4*z5*cos(z2)*cos(z3)2*z1*z4*z6* sin(z2)*sin(z3)2*z12*z5*z6*cos(z2)*sin(z3)))/z14(z7*cos(z3)*sin(z2)2*z1*z8*cos(z2)*cos(z3)+2*z4*z5*cos(z2)*cos(z3)+2*z1*z9*sin(z2)*sin(z3)2*z4*z6*sin(z2)*sin(z3)+2*z1*z52*cos(z3)*sin(z2)+2*z1*z62*cos(z3)*sin(z2)+4*z1*z5*z6*cos(z2)*(sin(z3))/z13; Jfx(8,2)=(z1*z7*cos(z2)*cos(z3)2*z42*cos(z2)*cos(z3)+z12*z8*cos(z3)*sin(z2)+z12*z9*cos(z2)*sin(z3)+z12*z52*cos(z2)*cos(z3)+z12*z62*cos(z2)*cos(z3) 2*z12*z5*z6*sin(z2)*sin(z3)2*z1 *z4*z5*cos(z3)*sin(z2)2*z1*z4*z6*cos(z2)*sin(z3))/z3; Jfx(8,3)=(z1*z7*sin(z2)*sin(z3)2*z42*sin(z2)*sin(z3)z12*z8*cos(z2)*sin(z3)z12*z9*cos(z3)*sin(z2)+z12*z52*sin(z2)*sin(z3)+z12*z62*sin(z2)*sin(z3)+2*z1*z4*z5*cos(z2)*sin(z3) +2*z1*z4*z6*cos(z3)*sin(z2)2*z12*z5*z6*cos(z2)*cos(z3))/z13; Jfx(8,4)=(4*z4*cos(z3)*sin(z2)2*z1*z5*cos(z2)*cos(z3)+2*z1*z6*sin(z2)*sin(z3))/z13; Jfx(8,5)=(2*z1*z4*cos(z2)*cos(z3)+2*z12*z5*cos(z3)*sin(z2)+2*z12*z6*cos(z2)*sin(z3))/z13; Jfx(8,6)=(2*z12*z5*cos(z2)*sin(z3)2*z1*z4*sin(z2)*sin(z3)+2*z12*z6*cos(z3)*sin(z2))/z13; Jfx(8,7)=(cos(z3)*sin(z2))/z12; Jfx(8,8)=(cos(z2)*cos(z3))/z1; Jfx(8,9) =-(sin(z2)*sin(z3))/z1;

[0112] Jfx(9,1)=(3*(z1*z7*sin(z3)2*z42*sin(z3)z12*z9*cos(z3)+z12*z62*sin(z3)+2*z1*z4*z6*cos(z3)))/z14(z7*sin(z3)2*z1*z9*cos(z3)+2*z4*z6*cos(z3)+2*z1*z62*sin(z3))/z13; Jfx(9,3)=(z1*z7*cos(z3)2*z42*cos(z3)+z12*z9*sin(z3)+z12*z62*cos(z3)2*z1*z4*z6*sin(z3))/z13; Jfx(9,4)=(4*z4*sin(z3)2*z1*z6*cos(z3))/z13; Jfx(9,6)=(2*z1*z4*cos(z3)+2*z12*z6* sin(z3))/z12; Jfx(9,7)=-sin(z3)/z12; Jfx(9,9)=cos(z3)/z1; end.

[0113] Referring to FIG. 4, position estimation accuracy and error using a nine state angle only target state estimator in the X, Y, and Z axes according to the principles of the present disclosure is shown. Its robust and high precision accuracy performance shown in FIG. 4 (right hand side plot) is smaller than 1e-3 meter while the left hand side figure shows the true position of the target in 3-D. The estimation vs truth is almost identical (and not able to differentiate the difference between them by eyes, i.e., right on top of one another axis by axis).

[0114] Referring to FIG. 5, velocity estimation accuracy and error using a nine state angle only target state estimator in the X, Y, and Z axes according to the principles of the present disclosure is shown. More specifically, the excellent performance estimation accuracy of the velocity component of the target motion in the X, Y, Z axes (i.e., Vx, Vy, Vz) is shown in FIG. 5. On the left hand side of FIG. 5 is the truth velocity of the target vs the actual estimation velocity component of the TSE. Since the target is accelerating in the x (Ax only) direction, only Vx is increasing in time (from 0 seconds to 10 seconds with a starting velocity Vx at lm/s and growing to 6 m/s at 10 seconds instant.) The target velocity in other two axes (Y and Z) are having zero velocities (Vy and Vz are equal zeros) and the 9 state TSE estimate matching that zero velocity right after seconds (i.e., after 2 seconds, the estimate of Vy and Vz settling to a zero m/s values). On the right hand side of FIG. 5 is the plotting of the velocity estimation errors in 3 axes. The velocity errors are very small (i.e., much less than 1e-3 m/s) which indicates the 9 state EKF TSE is doing quite well in reconstructing the target velocity vector in all axes.

[0115] Referring to FIG. 6, acceleration estimation accuracy and error using a nine state angle only target state estimator in the X, Y, and Z axes according to the principles of the present disclosure is shown. More specifically, for the case of the target is accelerating at 0.5 m/s/s in the x direction (i.e., Ax only), the 9 state EKF TSE provides an accurate estimation of the target acceleration state vector as illustrated on FIG. 6. On Ay and Az, since the target is not accelerating in these two axes, the errors are extremely small (i.e., e-6 to e-8 magnitudes in m/s/s).

[0116] Referring to FIG. 7, a plot of eight potential targets, with six of the targets flagged to be hit (designated targets via weapon to target assignment subsystem), where multiple AO measurements of multiple targets are processed according to the principles of the present disclosure is shown. It is worth pointing out here that the 9 state AO TSE solution developed herein has demonstrated its accuracy and effectiveness in reconstructing the 8 targets (even in closely space targets situation) and delivered those highly accurate TSEs to guidance law and weapon target assignment subsystems to achieve a successful engagement of 6 weapons against 6 designated targets.

[0117] The computer readable medium as described herein can be a data storage device, or unit such as a magnetic disk, magneto-optical disk, an optical disk, or a flash drive. Further, it will be appreciated that the term memory herein is intended to include various types of suitable data storage media, whether permanent or temporary, such as transitory electronic memories, non-transitory computer-readable medium and/or computer-writable medium.

[0118] It will be appreciated from the above that the invention may be implemented as computer software, which may be supplied on a storage medium or via a transmission medium such as a local-area network or a wide-area network, such as the Internet. It is to be further understood that, because some of the constituent system components and method steps depicted in the accompanying Figures can be implemented in software, the actual connections between the systems components (or the process steps) may differ depending upon the manner in which the present invention is programmed. Given the teachings of the present invention provided herein, one of ordinary skill in the related art will be able to contemplate these and similar implementations or configurations of the present invention.

[0119] It is to be understood that the present invention can be implemented in various forms of hardware, software, firmware, special purpose processes, or a combination thereof. In one embodiment, the present invention can be implemented in software as an application program tangible embodied on a computer readable program storage device. The application program can be uploaded to, and executed by, a machine comprising any suitable architecture.

[0120] While various embodiments of the present invention have been described in detail, it is apparent that various modifications and alterations of those embodiments will occur to and be readily apparent to those skilled in the art. However, it is to be expressly understood that such modifications and alterations are within the scope and spirit of the present invention, as set forth in the appended claims. Further, the invention(s) described herein is capable of other embodiments and of being practiced or of being carried out in various other related ways. In addition, it is to be understood that the phraseology and terminology used herein is for the purpose of description and should not be regarded as limiting. The use of including, comprising, or having, and variations thereof herein, is meant to encompass the items listed thereafter and equivalents thereof as well as additional items while only the terms consisting of and consisting only of are to be construed in a limitative sense.

[0121] The foregoing description of the embodiments of the present disclosure has been presented for the purposes of illustration and description. It is not intended to be exhaustive or to limit the present disclosure to the precise form disclosed. Many modifications and variations are possible in light of this disclosure. It is intended that the scope of the present disclosure be limited not by this detailed description, but rather by the claims appended hereto.

[0122] A number of implementations have been described. Nevertheless, it will be understood that various modifications may be made without departing from the scope of the disclosure. Although operations are depicted in the drawings in a particular order, this should not be understood as requiring that such operations be performed in the particular order shown or in sequential order, or that all illustrated operations be performed, to achieve desirable results.

[0123] While the principles of the disclosure have been described herein, it is to be understood by those skilled in the art that this description is made only by way of example and not as a limitation as to the scope of the disclosure. Other embodiments are contemplated within the scope of the present disclosure in addition to the exemplary embodiments shown and described herein. Modifications and substitutions by one of ordinary skill in the art are considered to be within the scope of the present disclosure.