Data processing: measuring – calibrating – or testing – Measurement system – Measured signal processing
Reexamination Certificate
1999-08-27
2002-11-19
Hoff, Marc S. (Department: 2857)
Data processing: measuring, calibrating, or testing
Measurement system
Measured signal processing
C701S223000
Reexamination Certificate
active
06484131
ABSTRACT:
FIELD OF THE INVENTION
The present invention relates to apparatus for localization and tracking.
BACKGROUND OF THE INVENTION
The theory of non-linear filtering and its applications are discussed in:
H. J. Kushner, “Approximations to Optimal Nonlinear Filters”. IEEE Trans. A.C., Vol. AC-12, No. 5, October 1967;
A. Gelb, J. F. Kaspar, Jr., R. A. Nash, Jr., C. E. Price, and A. A. Southerland, Jr., “Applied Optimal Estimation”, M.I.T. Press, Cambridge, Mass., 1974;
B. D. O. Anderson, and J. B. Moore, “Optimal Filtering”, Prentice-Hall, Englewood Cliffs, N.J., 1979;
A. H. Jazwinski, “Stochastic Processes and Filtering Theory”, Academic Press, New York, 1971; and
M. S. Grewal, and A. P. Andrews, “Kalman Filtering”, Prentice-Hall, Upper Saddle River, N.J., 1993.
The Biot-Savart law is discussed in:
J. D. Jackson, “Classical Electrodynamics”, John Willey & Sons, New York, N.Y., 1975.
The application of Extended Kalman Filters (EKF) to tracking in the context of radar is discussed, for example, in U.S. Pat. Nos. 5,075,694, 4,179,696, 3,952,304 and 3,935,572. Other tracking systems are discussed, for example, in U.S. Pat. Nos. 5,095, 467 and 4,855,932.
The Kalman Filter is a standard tool for “data fusion” of different sensors. In U.S. Pat. No. 5,416,712 GPS signals and dead reckoning are combined by a Kalman Filter, and where the gyro bias is also calibrated. In U.S. Pat. No. 5,645,077 automatic drift compensation is discussed.
The disclosures of all publications mentioned in the specification and of the publications cited therein are hereby incorporated by reference.
SUMMARY OF THE INVENTION
The present invention seeks to provide a non-linear Kalman Filter tracker.
There is thus provided in accordance with a preferred embodiment of the present invention pose tracking apparatus operative to track the pose of a moving object based on magnetic flux measurements taken in the vicinity of the moving object, the pose tracking apparatus including a non-linear Kalman filter-based tracker operative to receive magnetic flux measurements performed in the vicinity of the moving object, to operate a non-linear Kalman-type filter on the measurements, thereby to generate information regarding the pose of the moving object, and a pose indicator operative to provide an output indication of the information regarding the pose of the moving object.
Further in accordance with a preferred embodiment of the present invention the non-linear tracker includes an EKF (extended Kalman filter).
Additionally in accordance with a preferred embodiment of the present invention, the non-linear filter operates on a state vector whose components include pose coordinates and first time-derivatives of the pose coordinates.
Further in accordance with a preferred embodiment of the present invention the pose coordinates include 3 spatial coordinates and 2 orientation coordinates.
Further in accordance with a preferred embodiment of the present invention the apparatus also includes a transmitter array, which may include less than six operative transmitters, inducing magnetic flux in the vicinity of the moving object.
Still further in accordance with a preferred embodiment of the present invention the non-linear tracker employs a Biot-Savart transformation from the pose of the moving object to the magnetic flux measurements taken in its vicinity.
Still further in accordance with a preferred embodiment of the present invention the step of employing the Biot-Savart transformation includes computing a function h of a state vector &xgr;, as follows:
h
(
ξ
)
=
C
0
R
3
(
3
A
1
A
2
R
2
-
A
3
)
where C
0
is a coefficient,
R is the distance between a detector detecting the magnetic flux measurements and a transmitter within the transmitter array; and
A
1
=&dgr;x sin(&thgr;
s
)cos(&phgr;
s
)+&dgr;y sin(&thgr;
s
)sin(&phgr;
s
)+&dgr;z cos(&thgr;
s
)
A
2
=&dgr;x sin(&thgr;
d
)cos(&phgr;
d
)+&dgr;y sin(&thgr;
d
)sin(&phgr;
d
)+&dgr;z cos(&thgr;
d
)
A
3
=sin(&thgr;
s
)cos(&phgr;
s
)sin(&thgr;
d
)cos(&phgr;
d
)+sin(&thgr;
s
)sin(&phgr;
s
)sin(&thgr;
d
)sin(&phgr;
d
)+cos(&phgr;
s
)cos(&phgr;
d
)
and wherein the pose of the detector is (x
d
, y
d
, z
d
, &thgr;
d
, &phgr;
d
) and the pose of the transmitter is (x
s
, y
s
, z
s
, &thgr;
s
, &phgr;
s
), and where &dgr;x, &dgr;y and &dgr;z denote the distance between the x, y and z components, respectively, of the detector's pose and the transmitter's pose.
Additionally in accordance with a preferred embodiment of the present invention the non-linear tracker approximates an elliptic integral, at least when the moving object is close to a transmitter within the transmitter array, by computing first and second terms of a Taylor series representing the elliptic integral.
Additionally in accordance with a preferred embodiment of the present invention, the approximated elliptic integral includes a correction to the above mentioned A
1
and A
3
,
A
1
→A
1
(1−&dgr;)
A
3
→A
3
(1−&eegr;)
δ
=
5
8
(
ρ
R
)
2
(
7
A
1
2
R
2
-
3
)
η
=
9
8
(
ρ
R
)
2
(
5
A
1
2
R
2
-
1
)
and where &rgr; is the radius of the transmitter.
Still further in accordance with a preferred embodiment of the present invention the orientation component of the pose of the moving object is represented by two angles, continuous over time &thgr;′ and &phgr;′, whose relationship with conventional polar coordinates &thgr; and &phgr; is as follows:
θ
=
{
θ
′
if
mod
(
θ
,
2
π
)
≤
π
-
θ
′
if
mod
(
θ
,
2
π
)
>
π
ϕ
=
{
ϕ
′
if
θ
=
θ
′
ϕ
′
+
π
if
θ
=
-
θ
′
Still further in accordance with a preferred embodiment of the present invention, in order to avoid singularity, a dynamic offset is described by the following transformation:
&thgr;=cos
−1
[cos(&thgr;′)cos(&phgr;′)]
&phgr;=cos
−1
[{square root over (cos
2
(&thgr;)+sin
2
(&phgr;′)cos
2
(&thgr;′))}]
where &thgr; and &phgr; include the orientation component of the moving object's pose after the dynamic offset, and &thgr;′ and &phgr; include the orientation component of the moving object's pose before the dynamic offset.
Still further in accordance with a preferred embodiment of the present invention the non-linear filter employs the following matrices and operations:
&xgr;
k
(−)=&PHgr;&xgr;
k−1
(+)
where k is a time index, &xgr;
k
(−) is a state vector predictor, &xgr;
k
(+) is a state vector corrector, and &PHgr; is a state transition matrix,
P
k
(−)=&PHgr;
P
k−1
(+)&khgr;
T
+Q
where P(−) is an estimate error covariance matrix predictor, P(+) is an estimate error covariance matrix corrector and Q is a process noise covariance matrix,
&AutoLeftMatch;
H
k
=
∂
h
(
ξ
→
)
∂
ξ
→
&RightBracketingBar;
&AutoRightMatch;
ξ
k
(
-
)
where h is a sensitivity function and &xgr; is a state vector,
K
k
=P
k
(−)
H
k
T
[H
k
P
k
(−)
H
k
T
+R
k
]
−1
where R
k
is a measurement noise covariance matrix,
&xgr;
k
(+)=&xgr;
k
(−)+
K
k
{&zgr;
k
−h[&xgr;
k
(−)]}
where &zgr; denotes the magnetic flux measurements taken in the vicinity of the moving object, and
P
k
(+)=[
I−K
k
H
k
]P
k
(−)
Still further in accordance with a preferred embodiment of the present invention the magnetic flux measurements may include less than six magnetic flux measurements in the vicinity of the moving object.
Additionally in accordance with a preferred embodiment of the present invention, the non-linear tracker is operative to time-vary a measurement-noise covariance matrix R
Albeck Yehuda
Amorai-Moriya Netzer
Itzkovich Mordechai
Spivak Boaz
Hoff Marc S.
Netmor Ltd.
Raymond Edward
Townsend and Townsend / and Crew LLP
LandOfFree
Localization and tracking system does not yet have a rating. At this time, there are no reviews or comments for this patent.
If you have personal experience with Localization and tracking system, we encourage you to share that experience with our LandOfFree.com community. Your opinion is very important and Localization and tracking system will most certainly appreciate the feedback.
Profile ID: LFUS-PAI-O-2971849