Covariance matrix technique for error reduction
View Patent ↗A method for filtering spatial error from a measurement vector is provided to correct for roll, pitch and yaw angular motion. The method includes the following operations: Establish an unstabilized body reference frame. Convert the measurement vector to an unstabilized state vector x U in the unstabilized body reference frame. Establish a stabilized East-North-Up (ENU) reference frame. Calculate an unstabilized pre-transform covariance matrix M U from position variance of the body reference frame. Measure roll, pitch and yaw in the body reference frame as respective angle values (r, p, w). Calculate a transform matrix T between the body reference frame and the ENU reference frame. Calculate a stabilized data vector x S =Tx U from the transform matrix and the unstabilized state vector. Calculate a measured angle error sensitivity matrix M A from the angle values. Calculate a tri-diagonal angle error component matrix M E with square values of angle variance of the body reference frame. Calculate a total error covariance matrix P S =M A M E M A T +TM U T T . Calculate a Kalman gain matrix for current time k+1 as K(k+1)=P(k+1|k)H T [HP(k+1|k)H T +P S ] −1 , where P(k+1|k) is predicted gain covariance matrix from previous time k to the current time (k+1), and H is measurement Jacobian. Finally, apply the Kalman gain matrix to a predicted state estimate for correcting the measurement vector x m .
1. An angle correction system for removing angle error of a radar system from a measurement vector x m acquired by an antenna reflected from a target of a radar beam to correct for roll, pitch and yaw angular motion, said system comprising:
a reflection signal acquisition instrument to obtain the measurement vector x m of the target reflected from the radar beam emitted by the radar system and received by the antenna;
a reference determiner to establish an unstabilized body reference frame at the antenna for the radar system and a stabilized East-North-Up (ENU) reference frame in relation to said body reference frame;
a motion instrument to measure roll, pitch and yaw in said body reference frame as respective angle values (r, p, w);
a vector converter to transform the measurement vector x m to an unstabilized state vector x U in said body reference frame;
a calculator to determine:
an unstabilized pre-transform covariance matrix M U from position variance of said body reference frame,
a transform matrix T between said body reference frame and said ENU reference frame,
a stabilized data vector x S =Tx U from said transform matrix and said unstabilized state vector,
a measured angle error sensitivity matrix M A from said angle values,
a tri-diagonal angle error component matrix M E with square values of angle variance of said body reference frame,
a total error covariance matrix P S =M A M E M A T +TM U T T ;
a Kalman gain matrix for current time k+1 as:
K(k+1)=P(k+1|k)H T [HP(k+1|k)H T +P S ] −1 , where P(k+1|k) is predicted gain covariance matrix from previous time k to said current time (k+1) and H is measurement Jacobian, and
a state estimate {circumflex over (X)}(k+1|k) from a dynamic motion model Φ and a previous estimate at time k;
a gain implementer to apply said Kalman gain matrix to said state estimate for correcting said measurement vector x m as a covariance correction to the radar system; and
a correction implementer to establish said stabilized data vector x S as a stability correction to the radar system for tracking the target.
2. The system according to claim 1 , wherein said stabilized data vector x S =Tx U is determined as:
x
S
=
[
x
S
y
S
z
S
]
=
[
T
11
T
12
T
13
T
21
T
22
T
23
T
31
T
32
T
33
]
[
x
U
y
U
z
U
]
=
[
g
11
+
g
12
+
g
13
g
21
+
g
22
+
g
23
g
31
+
g
32
+
g
33
]
=
[
(
cos
p
cos
w
)
x
U
+
(
sin
r
sin
p
cos
w
-
cos
r
sin
w
)
y
U
+
(
cos
r
sin
p
cos
w
+
sin
r
sin
w
)
z
U
(
cos
p
sin
w
)
x
U
+
(
sin
r
sin
p
sin
w
+
cos
r
cos
w
)
y
U
+
(
cos
r
sin
p
sin
w
-
sin
r
cos
w
)
z
U
(
-
sin
p
)
x
U
+
(
sin
r
cos
p
)
y
U
+
(
cos
r
cos
p
)
z
U
]
,
where (g 11 , g 12 , . . . , g 33 ) represent sensitivity terms.
3. The system according to claim 1 , wherein said measured angle error sensitivity matrix is determined as:
M
A
=
[
(
∂
g
11
∂
r
+
∂
g
12
∂
r
+
∂
g
13
∂
r
)
(
∂
g
11
∂
p
+
∂
g
12
∂
p
+
∂
g
13
∂
p
)
(
∂
g
11
∂
w
+
∂
g
11
∂
w
+
∂
g
13
∂
w
)
(
∂
g
21
∂
r
+
∂
g
22
∂
r
+
∂
g
23
∂
r
)
(
∂
g
21
∂
p
+
∂
g
22
∂
p
+
∂
g
23
∂
p
)
(
∂
g
21
∂
w
+
∂
g
22
∂
w
+
∂
g
23
∂
w
)
(
∂
g
31
∂
r
+
∂
g
32
∂
r
+
∂
g
33
∂
r
)
(
∂
g
31
∂
p
+
∂
g
32
∂
p
+
∂
g
33
∂
p
)
(
∂
g
31
∂
w
+
∂
g
32
∂
w
+
∂
g
33
∂
w
)
]
,
where said (g 11 , g 12 , . . . , g 33 ) sensitivity terms are partially differentiated against respective roll, pitch and yaw angles.
4. The system according to claim 1 , wherein said angle error component matrix is determined as:
M
E
=
[
σ
r
2
0
0
0
σ
p
2
0
0
0
σ
w
2
]
,
where σ r 2 , σ p 2 , and σ w 2 respectively express the variances of roll, pitch and yaw.