Method and system for universal guidance and control of automated machines

• US 6,704,619 B1
• Filed: 06/03/2003
• Issued: 03/09/2004
• Est. Priority Date: 05/24/2003
• Status: Active Grant

• Current Assignee: American GNC Corporation
• Original Assignee: American GNC Corporation
• Inventors: Coleman, Norm, Lin, Ching-Fang
• Primary Examiner(s): Cuchlinski, Jr., William A.
• Assistant Examiner(s): Marc, McDieunel
• Application Number:
  US10/453,963
• Time in Patent Office: 280 Days
• Field of Search: 700/177, 700/172, 700/126, 700/180, 700/187, 700/189, 700/245, 700/190, 700/255, 700/249, 700/257, 700/258, 700/259, 700/260, 701/23, 701/220, 701/221, 701/4, 701/200, 714/11, 714/819, 702/141, 600/595, 706/13, 114/382, 318/568.1, 318/568.11, 318/568.13, 318/568.14, 318/567
• US Class Current: 700/245
• CPC Class Codes: G05B 2219/37388 Acceleration or deceleration, inertial measurement
• CPC Class Codes: G05B 2219/39259 GPS to control robotic arm
• CPC Class Codes: G05B 2219/42064 Position, speed and acceleration
• CPC Class Codes: G05D 1/0255 using acoustic signals, e.g. ultra-sonic singals sonar systems designed for anti-collision purposes G01S15/93
• CPC Class Codes: G05D 1/0278 using satellite positioning signals, e.g. GPS

Abstract

A system for universal guidance and control of automated machines incorporates with an IMU (Initial Measuring Unit) installed at an end effector of a motion element of an automated machine, fast-response feedback control for both position and angle servo-loops (for the end effector) greatly decreases the operational time needed to complete a preplanned trajectory. In addition, the closed-control loop design provides stabilization and isolation of the end effector from external disturbances. This unique navigation solution is based upon the uses of a set of equations performing an open loop computation with the inertial data as its input. This formulation of equations requires a periodic update of the open loop solution in order to bind the growth of system errors. The source of this update is the automated machine position measurement derived from the mechanical sensors in the system.

Claims

1. A system for universal guidance and control of an automated machine comprising a motion element having an end effector, wherein said system comprises:

an inertial sensor package, which is installed at said end effector of said motion element, sensing a motion and motion changes of said end effector and providing a motion measurement of said end effector through a navigation processing of said inertial sensor package to obtain measurement data;

a GPS receiver for providing GPS positioning measurements;

an AHRS/INS/GPS integration system receiving said motion measurement of said motion element and said GPS positioning measurements for imparting position and motion information;

an object detection system for ascertaining object presence;

an object tracking and guidance system processor receiving an information on a presence of objects of interest from said object detection system and a position and motion information from said AHRS/INS/GPS integration system to produce a guidance command as a command input to said central control processor;

a central control processor receiving said measurement data from said inertial sensor package, said position and motion information from said AHRS/INS/GPS integration system, and an output of said object tracking and guidance system processor, and comparing said measurement data with said command input to form error data which is received in said central control processor to produce a control signal in said central control processor; and

a motion actuator receiving said control signal from said central control processor to control speed force outputs of said motion actuator and driving said end effector of said motion element by said motion actuator according to said control signal, wherein errors between said motion being measured and said command input converges to zero, so as to ensure said end effector of said motion element moves along a trajectory as said command input requires.

2. The system, as recited in claim 1, wherein said motion actuator includes one or more electromechanical servo actuator valves and one or more hydraulic actuators which are controlled by said electromechanical servo actuator valves, wherein said central control processor receives output of said inertial sensor package and produces commands to said electromechanical servo actuator valves to control said hydraulic actuators for making said motion element move.

3. The system, as recited in claim 2, wherein said control signal is sent to said electromechanical servo actuator valves to control a hydraulic flow to said hydraulic actuators so as to control speed and force outputs of said hydraulic actuators.

4. The system, as recited in claim 3, wherein said end effector of said motion element is driven by said hydraulic actuators according to said control signal.

5. The system, as recited in claim 1, further comprising an acceleration control loop which comprises one accelerometer of said inertial sensor package to measure real motion, a converter converting delta velocity data to acceleration data, a first limit restricting said magnitude of said force, a first comparator comparing said acceleration command and measured acceleration, a first amplifier for signal amplifying, and an integrator for signal interation, wherein a produced voltage signal is sent to said motion actuator for driving said end effector.

6. The system, as recited in claim 5, wherein said accelerometer of said inertial sensor package measures an acceleration of said end effector and produces said delta velocity data, wherein said delta velocity data is sent to said converter in said central control processor to convert to said acceleration data, wherein said acceleration data is inputted in said first limit so as to limit said acceleration data and produce acceleration commands which are compared with said measured acceleration to produce an acceleration error signal by said first comparator, wherein said acceleration error signal is simplified by said first amplifier to form an amplified signal which is then integrated by said integrator, wherein an output of said integrator is converted to an analog voltage signal which is sent to said motion actuator to produce force according to said analog voltage signal by said motion actuator to drive said end effector to move while said acceleration error converges to zero.

7. The system, as recited in claim 6, further comprising a velocity control loop which makes use of said acceleration control loop as an inner control loop, wherein said velocity processing control loop comprises said inertial sensor package and a navigation module thereof for obtaining a real velocity of said end effector, a second limit for restricting said magnitude of said velocity, a second comparator for comparing a velocity command and said measured velocity, and a second amplifier for signal amplifying, wherein velocity processing produced data are sent to said acceleration control loop as said input acceleration command for driving said end effector, wherein said velocity control loop acts as an integrator to transform acceleration to velocity.

8. The system as recited in claim 7, wherein said velocity of said end effector is measured by said navigation module in said inertial sensor package, wherein output data of said inertial sensor package is processed by using said navigation module to produce velocity measurement of said end effector, wherein an input velocity signal is limited by said second limit to produce limited velocity data wherein said limited velocity data is compared with said measured velocity from said inertial sensor package by said second comparator to produce a velocity error signal, wherein said velocity error signal is amplified by said second amplifier, wherein an output of said second amplifier is sent to an input of said acceleration control loop to produce force by said motion actuator according to said input signal, wherein through said acceleration control loop and driving to said end effector said motion is generated while said velocity error converges to zero.

9. The system as recited in claim 8, further comprising a position control loop which makes use of said velocity control loop as an inner loop wherein said position processing loop comprises said inertial sensor package and a processing for obtaining a real position of said end effector, a third limit for restricting said magnitude of said position, a third comparator for comparing said position command and measured position, and a third amplifier for error signal amplifying, wherein position processing produced data are sent to said velocity control loop as an input velocity command for driving said end effector in which said position loop acts as an integrator to transform velocity to position.

10. The system, as recited in claim 9, wherein a position of said end effector is measured by said inertial sensor package so as to estimate said position by using a fixed lever arm parameter of said inertial sensor package, wherein said output data of said inertial sensor package is processed to produce a position measurement of said end effector, wherein said position measurement is limited by said third limit to produce limited position data, wherein said limited position data is compared with said measured position from said inertial sensor package by said third comparator to produce a position error signal which is amplified by said third amplifier, wherein an output of said third amplifier is sent to an input of said velocity control loop, wherein through said velocity control loop, said motion actuator produces force according to said input signal and drives said end effector to move while said position error converges to zero.

11. The system as recited in claim 10, further comprising an angular rate control loop;

which comprises said inertial sensor package and gyros thereof for obtaining a real angular rate of said end effector, a fourth limit for restricting a magnitude of an angular rate, a fourth comparator for comparing said angular rate command and measured angular rate, and a fourth amplifier for signal amplifying, wherein angular rate processing produced data are sent to said end effector for driving said end effector, wherein said angular rate control loop acts as an integrator to transform angular acceleration to said angular rate.

12. The system, as recited in claim 11, wherein an angular motion of said end effector is measured by said gyros in said inertial sensor package to output angular data in forms of delta angles, wherein said delta angle data is converted by an angular rate converter to angular rate data which is limited by said fourth limit to limited angular rate data, wherein said limited angular rate data in compared with said measured angular rate from said angular rate converter by said fourth comparator to produce an angular rate error signal, wherein said angular rate error signal is amplified by said fourth amplifier, wherein an output of said fourth amplifier is converted to analog signal and sent to said input of said motion actuator to produce torque and force that exerts on said end effect by said motion actuator and produce an angular acceleration that makes said angular rate error converges to zero, wherein said angular acceleration is accumulated by said angular rate control loop to produce said angular rate.

13. The system, as recited in claim 12, further comprising an angle control loop which makes use of said angular rate control loop as an inner loop, wherein said angle control loop comprises said inertial sensor package and an AHRS (Altitude. Heading Reference System) module for obtaining a real angle of said end effector, a fifth limit for restricting said magnitude of said angle a fifth comparator for comparing said angle command and measured angle, and a fifth amplifier for signal amplifying, wherein angle processing produced data are sent to said angular rate control loop for driving said end effector in which said angle loop acts as an integrator to transform said angular rate to angle.

14. The system, as recited in claim 13, wherein an angular motion of said end effector is measured by said inertial sensor package, wherein output data of said gyros is processed by said AHRS (Altitude Heading Reference System) module to produce angle data of said end effector which is limited by said fifth limit to produces limited angle data, wherein said limited angle data is compared with said measured angle from said inertial sensor package by said fifth, wherein an output of said fifth amplifier is sent to said angular rate control loop to produce a torque and force by said angular rate control loop that exerts on said end effect and produces said angular rate that makes an angle error converges to zero.

15. The system, as recited in claim 1, further comprising an angular rate control loop which comprises said inertial sensor package and gyros thereof for obtaining a real angular rate of said end effector, a fourth limit for restricting a magnitude of an angular rate, a fourth comparator for comparing said angular rate command and measured angular rate, and a fourth amplifier for signal amplifying, wherein angular rate processing produced data are sent to said end effector for driving said end effector, wherein said angular rate control loop acts as an integrator to transform angular acceleration to said angular rate.

16. The system, as recited in claim 15, wherein an angular motion of said end effector is measured by said gyros in said inertial sensor package to output angular data in form of delta angles, wherein said delta angle data is converted by an angular rate converter to angular rate data which is limited by said fourth limit to form limited angular rate data, wherein said limited angular rate data in compared with said measured angular rate from said angular rate converter by said fourth comparator to produce an angular rate error signal, wherein said angular rate error signal is amplified by said fourth amplifier, wherein an output of said fourth amplifier is converted to an analog signal and sent to said input of said motion actuator to produce torque and force that exerts on said end effector by said motion actuator and produces an angular acceleration that makes said angular rate error converges to zero, wherein said angular acceleration is accumulated by said angular rate control loop to produce said angular rate.

17. The system, as recited in claim 16, farther comprising an angle control loop which makes use of said angular rate control loop as an inner loop, wherein said angle control loop comprises said inertial sensor package and an AHRS (Altitude Heading Reference System) module for obtaining a real angle of said end effector, a fifth limit for restricting said magnitude of said angle, a fifth comparator for comparing said angle command and measured angle, and a fifth amplifier for signal amplifying, wherein angle processing produced data are sent to said angular rate control loop for driving said end effector in which said angle loop acts as an integrator to transform said angular rate to angle.

18. The system as recited in claim 17, wherein an angular motion of said end effector is measured by said inertial sensor package, wherein output data of said gyros is processed by said AHRS (Altitude Heading Reference System) module to produce angle data of said end effector which is limited by said fifth limit to produces limited angle data, wherein said limited angle data is compared with said measured angle from said inertial sensor package by said fifth comparator to produce an angle error signal which is amplified by said fifth amplifier, wherein an output of said fifth amplifier is sent to said angular rate control loop to produce a torque and force by said angular rate control loop that exerts on said end effector and produces said angular rate that makes an angle error converges to zero.

19. The system, as recited in claim 1, further comprising:

a second AHRS/INS/GPS integration system, installed at a base of a vehicle, providing an accurate reference position;

acoustic receivers affixed at known locations on said vehicle;

an acoustic transmitter mounted at said end effector, providing relative range from said acoustic transmitter (at said end effector) to said acoustic receivers mounted on said vehicle, wherein both absolute and relative positions of said end effector are determined using said measured range values; and

a down-looking sonar at said end effector, detecting ground, thus aiding pallet pick-up operations as well as enhancing safety features.

20. The system, as recited in claim 19, wherein said acoustic transmitter is an omni-directional device.

21. The system, as recited in claim 19, wherein a plurality of receivers are placed around said vehicle and a position determination of said end effector is based on a triangulation principle, using three or more measured ranges to different reference locations, wherein larger baselines imply better accuracy.

22. The system, as recited in claim 21, wherein acoustic ranging uses a time delay of an acoustic signal traveling from said acoustic transmitter to said acoustic receivers.

23. The system, as recited in claim 19, wherein said object detection system is a data link.

24. The system, as recited in claim 19, wherein said object detection system is selected from a group consisting of an imager, including laser scanner, and a LDRI (laser dynamic range sensor).

25. The system, as recited in claim 24, wherein said stereo cameras comprises two cameras and a vision software, combining a complete algorithmic suite from detection to recognition/tracking, wherein input from said two cameras is processed to detect the features on both camera images and calculate the range using feature disparities between the two images and known camera resolution, focal length, and baseline.

26. The system, as recited in claim 24, wherein by selecting a desired pair of camera inputs for processing, said vision software which is set up for two simultaneous camera inputs is applied to a 3-camera configuration.

27. The system, as recited in claim 24, wherein said stereo cameras further comprises first, second and third cameras and a vision software, combining a complete algorithmic suite from detection to recognition/tracking, wherein input from said three cameras is processed to detect the features on both camera images and calculate the range using feature disparities between the two images and known camera resolution, focal length, and baseline, wherein 3 camera inputs are simultaneous accommodated and a 3-camera system is selectively configured to provide accurate ranging at close-up and extended ranges, wherein close-up ranging is accomplished by establishing stereo correspondence for either said first camera and said second camera in pair or said second camera and said third camera in pair;

at longer ranges, said first camera and said third camera in pair which has a twice baseline distance are employed.

28. The system, as recited in claim 19, wherein said object detection system is a sensor selected from a group consisting of radar, laser, ladar, sonar, infrared, video, stereo cameras, and acoustic sensor, which is capable of executing full/partial coverage of the surrounding views.

29. The system, as recited in claim 28, wherein by selecting a desired pair of camera inputs for processing, said vision software which is set up for two simultaneous camera inputs is applied to a 3-camera configuration.

30. The system, as recited in claim 1, wherein said object detection system is a data link.

31. The system, as recited in claim 1, wherein said object detection system is selected from a group consisting of an imager, including laser scanner, and a LDRI (laser dynamic range sensor).

32. The system, as recited in claim 31, wherein said stereo cameras comprises two cameras and a vision software, combining a complete algorithmic suite from detection to recognition/tracking, wherein input from said two cameras is processed to detect the features on both camera images and calculate the range using feature disparities between the two images and known camera resolution, focal length, and baseline.

33. The system, as recited in claim 31, wherein said stereo cameras further comprises first, second and third cameras and a vision software, combining a complete algorithmic suite from detection to recognition/tracking, wherein input from said three cameras is processed to detect the features on both camera images and calculate the range using feature disparities between the two images and known camera resolution, focal length, and baseline, wherein 3 camera inputs are simultaneous accommodated and a 3-camera system is selectively configured to provide accurate ranging at close-up and extended ranges, wherein close-up ranging is accomplished by establishing stereo correspondence for either said first camera and said second camera in pair or said second camera and said third camera in pair;

at longer ranges, said first camera and said third camera in pair which has a twice baseline distance are employed.

34. The system, as recited in claim 1, wherein said object detection system is a sensor selected from a group consisting of radar, laser, ladar, sonar, infrared, video, stereo cameras, and acoustic sensor, which is capable of executing full/partial coverage of the surrounding views.

35. The system, as recited in claim 1, wherein said object tracking and guidance system processor is further implemented, wherein command signals for guidance and control results from a continuous guidance and control estimation, target image processing interfaces with Target/Sensor Knowledge Base to enhance probability of target detection while Recognition &

Tracking Update interfaces with Decision Knowledge Base to ascertain characteristics of said target allowing its recognition and accurate tracking of its motion, wherein said generic command signals for guidance and control from said continuous guidance and control estimation are fed into a guidance law module, wherein said generic command signals are further processed by a guidance law module to produce specific commands for motion controllers or control loops.

36. The system, as recited in claim 35, wherein said guidance law module further produces five command signals for the motion controllers or control loops, acceleration command for acceleration control loop, velocity command for velocity control loop;

position command for position control loop;

angular rate command for angular rate control loop, and angular position (angle) command for angular position control loop.

37. A method of universal guidance and control of an automated machine which comprises a motion element having an end effector, wherein said method comprises the steps of:

(a) sensing a motion and motion changes of said end effector of said motion element by an inertial sensor package installed at said end effector to form inertial measurement data;

(b) providing an accurate motion measurement of said end effector of said motion element through an AHRS/INS/GPS integration system by said inertial sensor package and a GPS receiver;

(c) sending said inertial measurement data from said inertial sensor package to a central control processor;

sending said accurate motion measurement from said AHRS/INS/GPS integration system to said central control processor, producing an object presence and position and motion information by an object detection system and an object tracking and guidance system processor to form a command input for said central control processor;

(d) receiving said inertial measurement data from said inertial sensor package by said central control processor;

receiving said accurate motion measurement from said AHRS/INS/GPS integration system, and receiving said object presence and position and motion information from said object detection system and said object tracking and guidance system processor by said central control processor;

(e) comparing said measurement data with said command input to form error data;

(f) receiving said error data in said central control processor;

(h) producing a control signal by using a controller algorithm in said central control processor;

(i) sending said control signal to a motion actuator to control speed and force outputs of motion actuator; and

(j) driving said end effector of said motion element by said motion actuator according to said control signal, wherein an error between said measured motion and said command input of said motion actuator converges to zero, so as to ensure said end effector of said motion element moves along a trajectory as said command input requires.

38. The method, as recited in claim 37, wherein said motion actuator comprises one or more hydraulic actuators and one or more electromechanical servo valves, wherein said control signal is sent to said electromechanical servo actuator valves to control a hydraulic flow to said hydraulic actuators so as to control speed and force outputs of said hydraulic actuators.

39. The method, as recited in claim 37, further comprising an acceleration control loop which comprises the steps of:

(a.1) measuring an acceleration of said end effector and producing delta velocity data by an accelerometer provided in said inertial sensor package;

(a.2) sending said delta velocity data to a converter in said central control processor;

(a.3) converting said delta velocity data to acceleration data;

(a.4) inputting and limiting said acceleration data with a first limit and producing limited acceleration commands;

(a.5) comparing each of said limited acceleration commands with said measured acceleration and producing an acceleration error signal by a first comparator;

(a.6) simplifying said acceleration error signal by a first amplifier and then integrating said amplified signal by an integrator;

(a.7) converting an output of said integrator to an analog voltage signal and sending said analog voltage signal to said motion actuator; and

(a.8) producing a force according to said analog voltage signal by said motion actuator and driving said end effector to move while an acceleration error converges to zero.

40. The method, as recited in claim 39, further comprising a velocity control loop which makes use of said acceleration control loop as an inner control loop, wherein said velocity control comprises the steps of:

(b.1) measuring a velocity of said end effector by a navigation module of said inertial sensor package;

(b.2) processing said output data of said inertial sensor package by using said navigation and producing velocity measurements of said end effector;

(b.3) limiting said velocity measurements by a second limit and producing limited velocity data;

(b.4) comparing said limited velocity data with said measured velocity from said inertial sensor package by a second comparator and producing a velocity error;

(b.5) amplifying said velocity error signal by a second amplifier;

(b.6) sending an output of said second amplifier to an input of said acceleration control loop; and

(b.7) producing a force by said motion actuator according to said input signal, wherein through said acceleration control loop and driving to said end effector, a motion is generated while said velocity error converges to zero.

41. The method, as recited in claim 40;

further comprising a position control loop which makes use of said velocity control loop as an inner loop, wherein said position control loop comprises the steps of;

(c.1) measuring a position of said end effector by said inertial sensor package

(c.2) estimating said position by using a fixed lever arm parameter;

(c.3) processing said output of said inertial sensor package by using navigation algorithms and producing a position measurement of said end effector;

(c.4) limiting said position measurement by a third limit and producing limited position data;

(c.5) comparing said limited position data with said measured position from said inertial sensor package by a third comparator and producing a position error signal;

(c.6) amplifying said position error signal by a third amplifier; and

(c.7) sending an output of said third amplifier to an input of said velocity control loop, wherein through said velocity control loop, said motion actuator produces a force to drive said end effector to move while said position error converges to zero.

42. The method, as recited in claim 41, further comprising an angular rate control loop which comprises the steps of:

(d.1) measuring an angular motion of said end effector by gyros provided in said inertial sensor package;

(d.2) outputting angular data of said gyros in form of delta angles;

(d.3) converting said delta angle data to angular rate data by an angular rate converter;

(d.4) limiting said angular rate data by a fourth limit and producing limited angular rate data;

(d.5) comparing said limited angular rate data with said measured angular rate from said angular rate converter by a fourth comparator and producing an angular rate error signal;

(d.6) amplifying said angular rate error signal by a fourth amplifier;

(d.7) converting an output of said fourth amplifier to an analog signal and sending said analog signal to an input of said motion actuator; and

(d.8) producing a torque and force that exerts on said end effect by said motion actuator and producing an angular acceleration that makes said angular rate error converges to zero.

43. The method, as recited in claim 42, further comprising an angle control loop which makes use of said angular rate control loop as an inner loop, wherein said angle control loop comprises the steps of:

(e.1) measuring an angular motion of said end effector by said inertial sensor package;

(e.2) processing said output data of said gyros by an AHRS (Altitude Heading Reference System) module provided in said inertial sensor package and producing angle data of said end effector;

(e.3) limiting said angle data by a fifth limit and producing limited angle data;

(e.4) comparing said limited angle data with said measured angle from said inertial sensor package by a fifth comparator and producing an angle error signal;

(e.5) amplifying said angle error signal by a fifth amplifier;

(e.6) sending an output of said fifth amplifier to said angular rate control loop; and

(e.7) producing a torque and force by said angular rate control loop that exerts on said end effect and producing an angular rate that makes said angle error converges to zero.

44. The method, as recited in claim 37, further comprising an angular rate control loop which comprises the steps of:

(d.1) measuring an angular motion of said end effector by gyros provided in said inertial sensor package;

(d.2) outputting angular data of said gyros in form of delta angles;

(d.3) converting said delta angle data to angular rate data by an angular rate converter;

(d.4) limiting said angular rate data by a fourth limit and producing, limited angular rate data;

(d.5) comparing said limited angular rate data with said measured angular rate from said angular rate converter by a fourth comparator and producing an angular rate error signal;

(d.6) amplifying said angular rate error signal by a fourth amplifier;

(d.7) converting an output of a fourth amplifier to an analog signal and sending said analog signal to an input of said motion actuator; and

(d.8) producing a torque and force that exerts on said end effect of said motion actuator and producing an angular acceleration that makes said angular rate error converges to zero.

45. The method, as recited in claim 44, further comprising an angle control loop which makes use of said angular rate control loop as an inner loop, wherein said angle control loop comprises the steps of:

(e.1) measuring an angular motion of said end effector by said inertial sensor package;

(e.2) processing said output data of said gyros by an AHRS (Altitude Heading Reference System) module provided in said inertial sensor package and producing angle data of said end effector;

(e.3) limiting said angle data by a fifth limit and producing limited angle data;

(e.4) comparing said limited angle data with said measured angle from said inertial sensor package by a fifth comparator and producing an angle error signal;

(e.5) amplifying said angle error signal by a fifth amplifier;

(e.6) sending an output of said fifth amplifier to said angular rate control loop; and

(e.7) producing a torque and force by said angular rate control loop that exerts on said end effect and producing an angular rate that makes said angle error converges to zero.

46. The method, as recited in claim 37 wherein said object presence is produced by two stereo cameras and the step (d) further comprises:

(d.1) preprocessing images from either said stereo cameras or image files to get rid of noise and enhance features thereof by a preprocessing module;

(d.2) performing segmentation to get a profile of said objects by a segmentation module;

(d.3) detecting a certain object by a detection module,

(d.4) performing recognition by a recognition module, and

(d.5) tracking said certain object by a tracking module.

47. The method, as recited in claim 46, wherein said preprocessing module is implemented by one or more of Median Filter, Histogram Equalization and Inverse Image.

48. The method, as recited in claim 46, wherein said segmentation module is implemented by one or more of Threshold Black/white, Suppress Black, Suppress White and Sobel Filter.

49. The method, as recited in claim 46, wherein said detection module is implemented by one or more of Line Detection, Circle Detection, Corner Detection and Gabor Filter.

50. The method, as recited in claim 46, wherein said recognition module is implemented by one or more of Match Filter, Graph Matching and Corner Classifier.

51. The method, as recited in claim 46, wherein said tracking module is implemented Peak Tracking or Centroiding Tracking.