Self-contained positioning method and system thereof for water and land vehicles

Claim 2

A self-contained positioning system of a vehicle, comprising: an inertial navigation system (INS), for sensing a motion of said vehicle to output inertial positioning data including position, velocity, and attitude data and receiving optimal estimates of said inertial positioning data from an integration processor to output compensated inertial positioning data; a magnetic sensor, for sensing an earth's magnetic field to measure a heading of said vehicle to output a magnetic heading measurement; an odometer, for measuring a distance traveled when said vehicle is on land; and a velocimeter, for measuring a vehicle speed when said vehicle is in water; wherein said integration processor derives said optimal estimates of said inertial positioning data by integrating said inertial positioning data, said magnetic heading measurement, said distance traveled, and said vehicle speed and feeds back said optimal estimates of said inertial positioning data to said inertial navigation system to calibrate ever increasing inertial positioning errors.

Claim 3

The system, as recited in claim 1 , wherein said magnetic sensor is a flux valve.

Claim 4

The system, as recited in claim 1 , wherein said magnetic sensor is a magnetometer.

Claim 5

The system, as recited in claim 1 , wherein said inertial navigation system further comprises: an inertial measurement unit (IMU), for sensing said motion of said vehicle to output digital angular increments and velocity increments signals in response to said motion of said vehicle and an INS processor, for receiving said digital angular increments and velocity increments signals from said IMU and said optimal estimates of said inertial positioning data from said integration processor to output said compensated inertial positioning data.

Claim 6

The system, as recited in claim 4 , wherein said INS processor runs: a sensor compensation module, for calibrating errors of said digital angular increments and velocity increments signals; and an inertial navigation algorithm module, for computing said inertial positioning data.

Claim 7

The system, as recited in claim 5 , wherein said inertial navigation algorithm module further comprises: a strapdown mathematical platform computation module, for computing a transformation matrix from a body frame to a navigation frame using said digital angular increments and extracting attitude angles from said transformation matrix and compensating errors of said attitude angles using optimal estimates of errors of said attitude data of said inertial positioning data; a specific force equation module, for transforming said digital velocity increments expressed in said body frame to said navigation frame using said transformation matrix and computing said velocity data of said inertial positioning data and compensating errors of said velocity data of said inertial positioning data using optimal estimates of errors of said velocity data of said inertial positioning data; and a location equation module, for integrating said velocity of said positioning data to obtain said position data of said inertial positioning data and compensating errors of said position data of said inertial positioning data using optimal estimates of errors of said position data of said inertial positioning data.

Claim 8

The system, as recited in claim 1 , wherein said integration processor further comprises: a magnetic sensor processing module, for producing a heading angle; an odometer processing module and a velocimeter processing module, for producing relative position error measurements for a Kalman filter; and a zero velocity update module, for determining if said vehicle stops automatically to perform zero velocity update of said Kalman filter; wherein said Kalman filter estimates errors of said inertial positioning data by means of performing Kalman filtering computation to calibrate inertial positioning measurement errors.

Claim 9

A system for a water and land vehicle, as recited in claim 4 , wherein said integration processor further comprises: a magnetic sensor processing module, for producing a heading angle; an odometer processing module and a velocimeter processing module, for producing relative position error measurements for a Kalman filter; and a zero velocity update module, for determining if said vehicle stops automatically to perform zero velocity update of said Kalman filter; wherein said Kalman filter estimates errors of said inertial positioning data by means of performing Kalman filtering computation to calibrate inertial positioning measurement errors.

Claim 10

The system, as recited in claim 5 , wherein said integration processor further comprises: a magnetic sensor processing module, for producing a heading angle; an odometer processing module and a velocimeter processing module, for producing relative position error measurements for a Kalman filter; and a zero velocity update module, for determining if said vehicle stops automatically to perform zero velocity update of said Kalman filter; wherein said Kalman filter estimates errors of said inertial positioning data by means of performing Kalman filtering computation to calibrate inertial positioning measurement errors.

Claim 11

The system, as recited in claim 6 , wherein said integration processor further comprises: a magnetic sensor processing module, for producing a heading angle; an odometer processing module and a velocimeter processing module, for producing relative position error measurements for a Kalman filter; and a zero velocity update module, for determining if said vehicle stops automatically to perform zero velocity update of said Kalman filter; wherein said Kalman filter estimates errors of said inertial positioning data by means of performing Kalman filtering computation to calibrate inertial positioning measurement errors.

Claim 12

The system, as recited in claim 7 , wherein in order to determine whether said vehicle stops, said zero velocity update module comprises: an odometer change test module, for receiving an odometer reading to determine whether said vehicle stops or restarts; a system velocity change test module, for comparing a system velocity change between a current interval and a previous interval to determine whether said vehicle stops or restarts; a system velocity test module, for comparing a system velocity magnitude with a predetermined value to determine whether said vehicle stops or restarts; and an attitude change test module, for comparing a system attitude magnitude with a predetermined value to determine whether said vehicle stops or restarts.

Claim 13

The system, as recited in claim 8 , wherein in order to determine whether said vehicle stops, said zero velocity update module comprises: an odometer change test module, for receiving an odometer reading to determine whether said vehicle stops or restarts; a system velocity change test module, for comparing a system velocity change between a current interval and a previous interval to determine whether said vehicle stops or restarts; a system velocity test module, for comparing a system velocity magnitude with a predetermined value to determine whether said vehicle stops or restarts; and an attitude change test module, for comparing a system attitude magnitude with a predetermined value to determine whether said vehicle stops or restarts.

Claim 14

The system, as recited in claim 9 wherein in order to determine whether said vehicle stops, said zero velocity update module comprises: an odometer change test module, for receiving an odometer reading to determine whether said vehicle stops or restarts; a system velocity change test module, for comparing a system velocity change between a current interval and a previous interval to determine whether said vehicle stops or restarts; a system velocity test module, for comparing a system velocity magnitude with a predetermined value to determine whether said vehicle stops or restarts; and an attitude change test module, for comparing a system attitude magnitude with a predetermined value to determine whether said vehicle stops or restarts.

Claim 15

The system, as recited in claim 10 , wherein in order to determine whether said vehicle stops, said zero velocity update module comprises: an odometer change test module, for receiving an odometer reading to determine whether said vehicle stops or restarts; a system velocity change test module, for comparing a system velocity change between a current interval and a previous interval to determine whether said vehicle stops or restarts; a system velocity test module, for comparing a system velocity magnitude with a predetermined value to determine whether said vehicle stops or restarts; and an attitude change test module, for comparing a system attitude magnitude with a predetermined value to determine whether said vehicle stops or restarts.

Claim 16

The system, as recited in claim 11 , wherein said Kalman filter further comprises an error measurement formation module for forming measurements for a state estimator and a state estimator for computing said optimal estimates of said inertial positioning data.

Claim 17

The system, as recited in claim 12 , wherein said Kalman filter further comprises an error measurement formation module for forming measurements for a state estimator and a state estimator for computing said optimal estimates of said inertial positioning data.

Claim 18

The system, as recited in claim 13 , wherein said Kalman filter further comprises an error measurement formation module for forming measurements for a state estimator and a state estimator for computing said optimal estimates of said inertial positioning data.

Claim 19

The system, as recited in claim 14 , wherein said Kalman filter further comprises an error measurement formation module for forming measurements for a state estimator and a state estimator for computing said optimal estimates of said inertial positioning data.

Claim 20

The system, as recited in claim 15 , wherein said state estimator further comprises a horizontal filter for obtaining estimates of horizontal INS solution data errors and a vertical filter for obtaining estimates of vertical INS solution data errors.

Claim 21

The system, as recited in claim 16 , wherein said state estimator further comprises a horizontal filter for obtaining estimates of horizontal INS solution data errors and a vertical filter for obtaining estimates of vertical INS solution data errors.

Claim 22

The system, as recited in claim 17 , wherein said state estimator further comprises a horizontal filter for obtaining estimates of horizontal INS solution data errors and a vertical filter for obtaining estimates of vertical INS solution data errors.

Claim 23

The system, as recited in claim 18 , wherein said state estimator further comprises a horizontal filter for obtaining estimates of horizontal INS solution data errors and a vertical filter for obtaining estimates of vertical INS solution data errors.

Claim 24

A self-contained positioning method of a vehicle, comprising the steps of: (a) sensing a vehicle motion and producing digital angular increments and velocity increments signals in response to said vehicle motion by an inertial measurement unit, (b) sensing an earth's magnetic field to measure a heading angle of said vehicle by a magnetic sensor, (c) measuring a relative velocity of said vehicle relative to a ground by an odometer, (d) measuring said relative velocity of said vehicle relative to a water by a velocimeter, and (e) deducing position data in an integration processor, using said digital angular increments and velocity increments signals, said heading angle, said relative velocity of said vehicle relative to said ground, or said relative velocity of said vehicle relative to said water.

Claim 25

A self-contained positioning method of a vehicle, comprising the steps of: (a) sensing a vehicle motion and producing digital angular increments and velocity increments signals in response to said vehicle motion by an inertial measurement unit, (b) sensing an earth's magnetic field to measure a heading angle of said vehicle by a magnetic sensor, (c) measuring a relative velocity of said vehicle relative to a ground by an odometer, (d) measuring said relative velocity of said vehicle relative to a water by a velocimeter, and (e) deducing position data in an integration processor, using said digital angular increments and velocity increments signals, said heading angle, said relative velocity of said vehicle relative to said ground, or said relative velocity of said vehicle relative to said water.

Claim 26

The self-contained positioning method, as recited in claim 23 , wherein the step (e) further comprises the steps of: (e.1) computing inertial positioning measurements using said digital angular increments and velocity increments signals; (e.2) computing a heading angle using earth's magnetic field measurements; (e.3) creating a relative position error measurement in an odometer processing module using said relative velocity of said vehicle relative to said ground for a Kalman filter; (e.4) creating a relative position error measurement in a velocimeter processing module using said relative velocity of said vehicle relative to said water for said Kalman filter; and (e.5) estimating errors of inertial positioning measurements by means of performing Kalman filtering computation to calibrate inertial positioning measurements.

Claim 27

The self-contained positioning method, as recited in claim 24 , wherein the step (e.3) and (e.4) further comprises the steps of: transforming said relative velocity of said odometer or velocimeter expressed in a body frame into a navigation frame; comparing said relative velocity of said odometer or velocimeter with an IMU velocity to form a velocity difference; and integrating said velocity difference during a predetermined interval.

Claim 28

The self-contained positioning method, as recited in claim 24 , wherein the step (e.5) further comprises the steps of: performing motion tests to determine whether said vehicle stops to initiate a zero-velocity update, formulating measurement equations and time varying matrix for said Kalman filter, and computing estimates of error states using said Kalman filter.

Claim 29

The self-contained positioning method, as recited in claim 25 , wherein the step (e.5) further comprises the steps of: performing motion tests to determine whether said vehicle stops to initiate a zero-velocity update, formulating measurement equations and time varying matrix for said Kalman filter, and computing estimates of error states using said Kalman filter.