Patent Document ID: 7626534
Application ID: 11818041
Patent Flag: 1

Claim One:
1. A method for compensating for the positional and alignment errors of a sensor tracking a target with known acceleration, which sensor generates sensed information, said method comprising the steps of: defining the estimator state given by s _ = { X _ E ~ X. _ E ~ δ ⁢ ⁢ R _ E ~ δ ⁢ ⁢ θ _ } comprising the target position X {tilde over (E)} and velocity {dot over (X)} {tilde over (E)} and the positional bias δ R {tilde over (E)} of the sensor and the angular bias of the sensor δ θ ; applying to said sensed information any sensor positional bias update information and angular bias information to produce updated sensed information which ultimately provides improved target state information; propagating state estimates from the previous time t i-1 to the current time t i where the subscript i refers to the filter cycle iteration to produce time updated state estimates according to s ^ _ ⁡ ( t i ) = s ^ _ ⁡ ( t i - 1 ) + ∫ t i - 1 t i - 1 + Δ ⁢ ⁢ 1 ⁢ s. ^ _ ⁡ ( τ ) ⁢ ⁢ ⅆ τ ; computing the Jacobian of the state dynamics of the target according to J = [ ∂ s. _ ∂ s _ ] = [ ∂ s. _ ∂ X _ E ~ ⁢ ∂ s. _ ∂ X. _ E ~ ⁢ ∂ s _. ∂ δ ⁢ R _ E ~ ⁢ ∂ s _. ∂ δ ⁢ θ _ ] = [ 0 3 × 3 I 3 × 3 0 3 × 3 0 3 × 3 ∂ X _ ¨ E ~ ∂ X _ E ~ ∂ X _ ¨ E ~ ∂ X _. E ~ ∂ X _ ¨ E ~ ∂ δ ⁢ R _ E ~ ∂ X _ ¨ E ~ ∂ δ ⁢ θ _ 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ] where: ∂ X ¨ _ E ~ ∂ X _ E ~ = - μ ( Z _ ^ E ~ · Z _ ^ E ~ ) 3 2 ⁡ [ I 3 × 3 - 3 ( Z _ ^ E ~ · Z _ ^ E ~ ) ⁡ [ Z _ ^ E ~ · Z _ ^ E ~ T ] ] - 〚 ω ^ _ E ~ 〛 · 〚 ω ^ _ E ~ 〛 ∂ X ¨ _ E ~ ∂ X _. E ~ = - 2 · 〚 ω ^ _ E ~ 〛 ∂ X _ ¨ ∂ δ ⁢ R _ E = 0 3 × 1 ∂ X _ ¨ E ~ ∂ δ ⁢ θ _ = - μ ( Z _ ^ E ~ · Z _ ^ E ~ ) 3 2 ⁡ [ I 3 × 3 - 3 ( Z _ ^ E ~ · Z _ ^ E ~ ) ⁡ [ Z _ ^ E ~ · Z _ ^ E ~ T ] ] · 〚 R _ ^ E ~ 〛 · T ^ p ~ E ~ ⁡ ( t i ) - 2 · [ [ X. ^ _ E ~ ] ] · ⁢ ⁢ 〚 ω ^ _ E ~ 〛 · T ^ p ~ E ~ ⁡ ( t i ) + [ 〚 ω ^ _ E ~ 〛 · 〚 X ^ _ E ~ 〛 · 〚 ω ^ _ E ~ 〛 + 〚 ω ^ _ E ~ 〛 · 〚 R ^ _ E ~ 〛 · 〚 ω ^ _ E ~ 〛 + ⁢ ⁢ 〚 〚 ω ^ _ E ~ 〛 · X ^ _ E ~ 〛 · 〚 ω ^ _ E ~ 〛 + 〚 〚 ω ^ _ E ~ 〛 · R ^ _ E ~ 〛 · 〚 ω ^ _ E ~ 〛 - 〚 ω ^ _ E ~ 〛 · 〚 ω ^ _ E ~ 〛 · 〚 R ^ _ E ~ 〛 ] · T ^ p ~ E ~ ⁡ ( t i ) ⁢ ⁢ ⁢ and ⁢ ⁢ where _ ⁢  ⁢ R ^ _ E ~ = T ^ E E ~ · R ^ _ E ω _ ^ E ~ = T ^ E E ~ · ω _ E T ^ E E ~ = T P E ⁡ ( t i ) · [ I 3 × 3 + 〚 θ ^ _ 〛 ] · T E P ⁡ ( t i ) and where J is the Jacobian of the state dynamics of the target; s represents the sensor frame; underscore (_) represent a vector quantity; overcaret (^) represents an estimate of the argument; overtilde (˜) represents a bias term; ( ) T represents the transpose of the argument matrix; 0 3×3 is a 3×3 matrix in which all components are zero; I 3×3 is a 3×3 identity matrix (ones along the principal diagonal and zeroes elsewhere); Z is the vector sum of the sensor position vector with respect to the center of the Earth, and the relative position vector of the sensed object with respect to the sensor position; μ is the Earth's gravitational constant; ω is the Earth's angular velocity vector; T represents a coordinate system transformation matrix; P represents a covariance matrix of an estimated state vector; Q represents the process noise covariance matrix used by a Kalman filter; and the • notation denotes a skew symmetric matrix of the vector argument; computing, from said Jacobian, the state transition matrix for the extended Kalman filter algorithm 
 Φ≈ I+JΔt+ 0.5 J 2 Δt 2 where Φ is the state transition matrix truncated to include no more than second-order terms; I is a 12×12 identity matrix; J is a 12×12 Jacobian matrix; and using said state transition matrix, time propagating the covariance of a state vector comprising the position and velocity states of said target and the positional bias of the sensor given by 
 P ( t i )=Φ P ( t i-1 )Φ T +Q.