Pedometer navigator system
First Claim
1. A pedometer navigator system carried by a pedestrian having a left and right foot for calculating an SBPMS position displacement vector in a fixed navigational coordinate frame, having a north, east and down axis, comprising:
- an Euler Angle Measuring Subsystem (EAMS) having a right-handed or left-handed coordinate frame having an EAMS origin, the EAMS providing roll, pitch and azimuth Euler angle values for the right-handed or left-handed coordinate frame with respect to the north-east-down axes of the fixed navigation coordinate frame, a Short Baseline Position Measuring Subsystem (SBPMS) having an SBPMS coordinate frame having an SBPMS origin for measuring and providing a continuous series of time indexed left position vectors {right arrow over (ρ
)}leftp and right position vectors {right arrow over (ρ
)}rightp in SBPMS coordinate values characterizing the position of the left and right foot with respect to the SBPMS origin, the SBPMS coordinate frame being coupled to and in a fixed and predetermined alignment with the EAMS right-handed or left-handed coordinate frame, a computer means responsive to the roll, pitch and azimuth Euler angle values and the time indexed left and right position vectors for providing a first transition vector in fixed navigation coordinate frame values, characterizing the movement of the SBPMS origin for each interval that a left foot is stationary and the right foot is moving and a second transition vector in fixed navigation coordinate frame values characterizing the movement of the SBPMS origin for each interval that a right foot is stationary and the left foot is moving, the first and second transition vectors being added to form an SBPMS position displacement vector.
1 Assignment
0 Petitions
Accused Products
Abstract
A pedometer navigator system has an Euler Angle Measuring Subsystem (EAMS) coupled to a body frame provides Euler angles of a fixed coordinate system with respect to a navigational coordinate system. Left and right sensors are mounted on a surveyor'"'"'s feet. An SBPMS (short baseline position measuring subsystem) provides a left vector {right arrow over (ρ)}leftp and a right vector {right arrow over (ρ)}rightp characterizing the position of the sensors with respect to the SBPMS coordinate system origin. A computer means responds to the Euler angles and left and right vector and determines an interval during which the left or right foot is stationary, and providing a transition vector referenced to the earth fixed navigational coordinate system. A navigation algorithm adds each transition vector to the previous present position of the navigation reference system to obtain a next present position of the position measuring subsystem.
-
Citations
20 Claims
-
1. A pedometer navigator system carried by a pedestrian having a left and right foot for calculating an SBPMS position displacement vector in a fixed navigational coordinate frame, having a north, east and down axis, comprising:
-
an Euler Angle Measuring Subsystem (EAMS) having a right-handed or left-handed coordinate frame having an EAMS origin, the EAMS providing roll, pitch and azimuth Euler angle values for the right-handed or left-handed coordinate frame with respect to the north-east-down axes of the fixed navigation coordinate frame, a Short Baseline Position Measuring Subsystem (SBPMS) having an SBPMS coordinate frame having an SBPMS origin for measuring and providing a continuous series of time indexed left position vectors {right arrow over (ρ
)}leftp and right position vectors {right arrow over (ρ
)}rightp in SBPMS coordinate values characterizing the position of the left and right foot with respect to the SBPMS origin,the SBPMS coordinate frame being coupled to and in a fixed and predetermined alignment with the EAMS right-handed or left-handed coordinate frame, a computer means responsive to the roll, pitch and azimuth Euler angle values and the time indexed left and right position vectors for providing a first transition vector in fixed navigation coordinate frame values, characterizing the movement of the SBPMS origin for each interval that a left foot is stationary and the right foot is moving and a second transition vector in fixed navigation coordinate frame values characterizing the movement of the SBPMS origin for each interval that a right foot is stationary and the left foot is moving, the first and second transition vectors being added to form an SBPMS position displacement vector. - View Dependent Claims (2, 3, 4, 5, 6)
a body frame, and wherein, the EAMS and the SBPMS are coupled to the body frame, the EAMS and the SBPMS being positioned to align the EAMS right-handed or left-handed coordinate frame to be in fixed relation with the SBPMS coordinate frame.
-
-
3. The pedometer navigator system of claim 1 wherein the EAMS is selected from the group comprising:
an Attitude and Heading Reference System (AHRS), an Inertial Navigation System (INS) or an Aided Inertial Navigation System (AINS).
-
4. The pedometer navigator system of claim 1 wherein the computer means further comprises a stationary foot detection algorithm for generating
a both stationary signal indicating that both feet are stationary, a left stationary signal indicating that the left foot is stationary and a right stationary signal indicating that the right foot is stationary, the algorithm comprising the steps of: -
continuously calculating a time indexed direction cosine matrix (DCM), multiplying a time indexed left position vector {right arrow over (ρ
)}leftp and right position vector {right arrow over (ρ
)}rightp by the respective time indexed DCM to obtain a respective time indexed left time indexed relative position vector {right arrow over (ρ
)}leftg and right time indexed relative position vector {right arrow over (ρ
)}rightg in fixed navigation coordinate frame values,calculating a time indexed baseline vector by subtracting the left time indexed relative position vector from the right time indexed relative position vector, each time indexed baseline vector having a time indexed baseline vector north component Δ
ρ
N and an indexed baseline vector east component Δ
ρ
E,calculating an azimuth rotational rate of the time indexed baseline vector ω
Δ
ρ
for each time indexed baseline vector'"'"'s north component and east component, the rates of change Δ
{dot over (ρ
)}E and Δ
{dot over (ρ
)}N of the north and east components, the sum of squared values of each indexed baseline vector'"'"'s north component and east component, Δ
ρ
N2+Δ
ρ
E2, andcalculating the value of the azimuth rotational rate of the time indexed baseline vector ω
Δ
ρ
from the following equation for each pair of indexed baseline vector'"'"'s north component and east component,
the computer means being further characterized to providethe both stationary signal indicating a decision that neither foot is moving subject to a corresponding determination that the absolute magnitude of the rate of change of azimuth ω
Δ
ρ
is below a predetermined (and substantially zero) noise threshold, and providethe left stationary signal indicating a decision that the pedestrian'"'"'s left foot is moving forward with respect to his right foot in response to the rate of change of azimuth ω
Δ
ρ
exceeding the predetermined noise threshold and the sign of ω
Δ
ρ
being positive, i.e., ω
Δ
ρ
<
0, and providethe right stationary signal indicating a decision that the pedestrian'"'"'s right foot is moving forward with respect to his left foot in response to the rate of change of azimuth ω
Δ
ρ
exceeding the predetermined noise threshold and the sign of ω
Δ
ρ
being negative, i.e., ω
Δ
ρ
<
0 being negative.
-
-
5. The pedometer navigator system of claim 4 wherein the computer means is further characterized to calculate the SBPMS current position {right arrow over (r)}g (tcurrent) by the steps of:
-
adding all SBPMS position displacement vectors Δ
{right arrow over (ρ
)}1-3g from a starting time to the current time,summing the result of the previous step with the SBPMS starting position {right arrow over (r)}gstart, the sum being characterized by the equation
-
-
6. The pedometer navigator system of claim 1 wherein the computer means further comprises an algorithm for calculating the SBPMS current position {right arrow over (r)}g (tcurret) by the steps of:
-
using a left and right stationary foot detection algorithm for generating a both stationary signal indicating that both feet are stationary, a left stationary signal indicating that the left foot is stationary and a right stationary signal indicating that the right foot is stationary, and the steps for the left and right stationary foot detection algorithm comprising;
continuously calculating a time indexed DCM (a time indexed direction cosine matrix), multiplying each time indexed left position vector {right arrow over (ρ
)}leftp and right position vector {right arrow over (ρ
)}rightg by the respective DCM to obtain a respective left time indexed relative position vector {right arrow over (ρ
)}leftg and right time indexed relative position vector {right arrow over (ρ
)}rightg in fixed navigation coordinate frame values,calculating an indexed baseline vector by subtracting the left time indexed relative position vector from the right time indexed relative position vector, each indexed baseline vector having an indexed baseline vector north component Δ
ρ
N and an indexed baseline vector east component Δ
ρ
E,calculating an azimuth rotational rate of the time indexed baseline vector ω
Δ
ρ
for each time indexed baseline vector'"'"'s north component and east component, Δ
{dot over (ρ
)}e and Δ
{dot over (ρ
)}N, from the following equation for each pair of indexed baseline vector'"'"'s north component and east component,
the computer and algorithm means being further characterized to providethe both stationary signal indicating a decision that neither foot is moving subject to a corresponding determination that the absolute magnitude of the rate of change of azimuth ω
Δ
ρ
is below a predetermined (and substantially zero) noise threshold, and providethe left stationary signal indicating a decision that the pedestrian'"'"'s left foot is moving forward with respect to his right foot in response to the rate of change of azimuth ω
Δ
ρ
exceeding the predetermined noise threshold and the sign of ω
Δ
ρ
being positive, i.e., ω
Δ
ρ
<
0, and providethe right stationary signal indicating a decision that the pedestrian'"'"'s right foot is moving forward with respect to his left foot in response to the rate of change of azimuth ω
Δ
ρ
exceeding the predetermined noise threshold and the sign of ω
Δ
ρ
being negative, i.e., ω
Δ
ρ
<
0 being negative,the steps for calculating the SBPMS current position {right arrow over (r)}g (tcurrent) further comprising the steps of;
using each left stationary signal to mark times t1 and t2 and contemporaneously calculate a left indexed SBPMS left relative displacement vector Δ
{right arrow over (ρ
)}1-2g from
-
-
7. A pedometer navigator system carried by a pedestrian, comprising:
-
an Aided Inertial Navigation System (AINS) having a right-handed or left-handed coordinate frame having an AINS origin, the AINS providing roll, pitch and azimuth angle Euler angle values for the right-handed or left-handed coordinate frame with respect to a north-east-down axes of a fixed navigation coordinate frame, the AINS having a Kalman filter responsive to at least one aiding input, a Short Baseline Position Measuring Subsystem(SBPMS) having an SBPMS coordinate frame having an SBPMS origin for measuring and providing a series of time indexed position vectors {right arrow over (ρ
)}ip, each vector characterizing a position of at least a first indexed foot with respect to the SBPMS origin,the SBPMS coordinate frame being coupled to and in a fixed and predetermined alignment with the AINS right-handed or left-handed coordinate frame, a computer means for executing a program responsive to the roll, pitch and azimuth Euler angle values and the series of time indexed position vectors for providing a series of SBPMS left and right relative displacement vectors in fixed navigation coordinate frame values characterizing the movement of the SBPMS origin for each interval during which the respective indexed foot is stationary and the other indexed foot is moving, each SBPMS left and right relative displacement vector being output to the Kalman filter aiding input. - View Dependent Claims (8, 9, 10, 11, 12, 13, 14, 15, 16)
a lever arm vector from the AINS center to the SBPMS origin {right arrow over (l)}S-Pb, the computer means and further comprising;
a computer program for calculating indexed values of an inertial navigation solution displacement Δ
{right arrow over (r)}SNV1-2g using the equation;
for a left foot inertial navigation solution displacement, and or the equation;
for a right foot inertial navigation solutiondisplacement wherein {right arrow over (ν
)}SNVg represents the series of time indexed inertial navigator velocity NED components, {right arrow over (ω
)}Sb represents the series of body rates of the AINS and {right arrow over (l)}S-Pb represents the lever arm vector from the AINS center to the SBPMS origin.
-
-
9. The pedometer navigator system of claim 8 wherein the Kalman filter is coupled to receive and calculate the difference between the inertial navigation solution displacement and the series of SBPMS left and right relative displacement vectors for each foot, the differences being calculated from the equations:
-
10. The pedometer navigator system of claim 7 wherein the computer means further comprises:
-
an inertial navigation system referenced stationary foot detection algorithm for each foot having an SBPMS sensor, the algorithm comprising the steps of;
calculating a time indexed Direction Cosine Matrix (DCM) at time t, calculating a value for the absolute (right or left) foot position {right arrow over (r)}foot i g at a first time t for each foot having a sensor, the algorithm steps comprising;
multiplying a respective indexed position vector {right arrow over (ρ
)}ig by its respective DCM to obtain a respective time-indexed SBPMS relative position vector {right arrow over (ρ
)}ig in fixed navigation coordinate frame values at index time t,fetching an indexed geographic position of the SBPMS, {right arrow over (r)}SBPMSg, from the AINS at index time t, adding the indexed geographic position of the SBPMS, {right arrow over (r)}SBPMSg to the respective time indexed relative position vector {right arrow over (ρ
)}ig at index time t to obtain a value for {right arrow over (r)}footi g at index time t,multiplying the next respective indexed position vector {right arrow over (ρ
)}ip by its respective DCM to obtain a respective time indexed relative position vector {right arrow over (ρ
)}ig in fixed navigation coordinate frame values at index time t+Δ
t,fetching the next indexed geographic position of the SBPMS, {right arrow over (r)}SBPMSg, from the AINS at index time t+Δ
t,adding the indexed geographic position of the next SBPMS, {right arrow over (r)}SBPMSg to the respective next time indexed relative position vector {right arrow over (ρ
)}ig to obtain a value for {right arrow over (r)}footi g at time t+Δ
t,subtracting the value of {right arrow over (r)}foot i g (t) from {right arrow over (r)}footi g (t+Δ
t),calculating the absolute value of the vector difference, and generating a foot stationary signal for each foot subject to the condition;
-
-
11. The pedometer navigator system of claim 7 wherein the computer means foot detection algorithm predetermined time interval of claim 8 is a time interval of at least one second.
-
12. The pedometer navigator system of claim 7 wherein the computer means further comprises:
-
a stationary indexed foot detection algorithm responsive to the roll, pitch and azimuth Euler angle values, the series of time indexed position vectors, {right arrow over (ρ
)}ip, anda computed value of the indexed geographic position of the SBPMS, {right arrow over (r)}SBPMSg, from the AINS by calculating an absolute (right or left) indexed foot position {right arrow over (r)}foot i g in geographic coordinates using the equation
-
-
13. The pedometer navigator system of claim 10 wherein the difference value for successive indexed foot position are calculated and
a determination is made that the indexed foot is stationary on the condition that the absolute difference value of -
14. The pedometer navigator system of claim 7 wherein the computer means further comprises an algorithm for calculating a right foot stationary signal and a left foot stationary foot s signal for the right and left foot by the steps of:
-
continuously calculating a time indexed Direction Cosine Matrix (DCM), multiplying each time indexed left position vector {right arrow over (ρ
)}leftp and or right position vector {right arrow over (ρ
)}rightp by the respective DCM to obtain a respective left time indexed relative position vector {right arrow over (ρ
)}leftp and right time indexed relative position vector {right arrow over (ρ
)}rightp in fixed navigation coordinate frame values,calculating an indexed baseline vector by subtracting the left time indexed relative position vector from the right time indexed relative position vector, each indexed baseline vector having an indexed baseline vector north component Δ
ρ
N andan indexed baseline vector east component Δ
ρ
E,calculating the rotational rate of the baseline vector ω
Δ
ρ
for each indexed baseline vector'"'"'s north component and east component, the rates of change Δ
{dot over (ρ
)}E and Δ
{dot over (ρ
)}N of the north and east components, the sum of squared values of each indexed baseline vector'"'"'s north component and east component, Δ
ρ
N2+Δ
ρ
E2, andcalculating the value of ω
Δ
ρ
from the following equation for each pair of indexed baseline vector'"'"'s north component and east component,
the computer and algorithm means being further characterized to providea right stationary signal indicating that the pedestrian'"'"'s left foot is moving forward with respect to his right foot in response to the rate of change of azimuth ω
Δ
ρ
exceeding a predetermined noise threshold and the sign of ω
Δ
ρ
being positive, i.e., ω
Δ
ρ
<
0, and providea left stationary signal indicating that the pedestrian'"'"'s right foot is moving forward with respect to his left foot in response to the rate of change of azimuth ω
Δ
ρ
exceeding a predetermined noise threshold and the sign of ω
Δ
ρ
being negative, i.e., ω
Δ
ρ
<
0 being negative.
-
-
15. The pedometer navigator system of claim 12 wherein the computer means further comprises an algorithm for calculating an SBPMS position displacement vector for the left and right foot by the steps of:
using each left stationary signal to mark times t1 and t2 and contemporaneously calculate an SBPMS left relative displacement vector Δ
{right arrow over (ρ
)}1-2g from
-
16. The pedometer navigator system of claim 14 wherein the computer means further comprises:
-
an INS referenced stationary foot detection algorithm for each foot having an SBPMS sensor, the algorithm comprising the steps of;
calculating a time indexed DCM (a time indexed direction cosine matrix) valid at time t calculating a value for the absolute (right or left) foot position {right arrow over (r)}foot i g at a first time t for each foot having a sensor, the algorithm steps comprising;
multiplying a respective indexed position vector {right arrow over (ρ
)}ip by its respective DCM to obtain a respective time indexed relative position vector {right arrow over (ρ
)}ig in fixed navigation coordinate frame values at index time t,fetching an indexed geographic position of the SBPMS, {right arrow over (r)}SBPMSg, from the AINS at index time t, adding the indexed geographic position of the SBPMS, {right arrow over (r)}SBPMSg to the respective time indexed relative position vector {right arrow over (ρ
)}ig at index time t to obtain a value for {right arrow over (r)}footi g at index time t,multiplying the next respective indexed position vector {right arrow over (ρ
)}ip by its respective DCM to obtain a respective time indexed relative position vector {right arrow over (ρ
)}ig in fixed navigation coordinate frame values at index time t+Δ
t,fetching the next indexed geographic position of the SBPMS, {right arrow over (ρ
)}SBPMSg, from the AINS at index time t+Δ
t,adding the indexed geographic position of the next SBPMS, {right arrow over (r)}SBPMSg to the respective next time indexed relative position vector {right arrow over (r)}ig to obtain a value for {right arrow over (r)}foot i g at time t+Δ
t,subtracting the value of {right arrow over (r)}foot i g (t) from {right arrow over (r)}footi g (t+Δ
t),calculating the absolute value of the vector difference, and generating a foot stationary signal for each foot subject to the condition;
-
-
17. A pedometer navigator system carried by a pedestrian, comprising:
-
an Aided Inertial Navigation System (AINS) providing roll, pitch and azimuth Euler angle values, a Short Baseline Position Measuring Subsystem (SBPMS) and providing a series of time indexed position vectors {right arrow over (ρ
)}ip, characterizing the position of at least a first indexed foot with respect to the SBPMS origin,an SBPMS coordinate frame being coupled to the AINS, a computer means for executing a program responsive to the roll, pitch and azimuth Euler angle values and the (SBPMS,) series of time indexed position vectors for providing a series of SBPMS left and right relative displacement vectors characterizing the movement of the SBPMS origin as aiding inputs to a Kalman filter. - View Dependent Claims (18, 19, 20)
a right-handed or left-handed coordinate frame having an AINS origin, the AINS providing roll, pitch and azimuth angle values for the right-handed or left-handed coordinate frame with respect to a north-east-down axes of a fixed navigation coordinate frame and wherein, the SBPMS (short baseline position measuring subsystem) has an SBPMS coordinate frame having a SBPMS origin for measuring and providing a series of time indexed position vectors {right arrow over (ρ
)}ip, each vector characterizing the position of at least a first indexed foot with respect to the SBPMS origin,the SBPMS coordinate frame being coupled to and in a fixed and predetermined alignment with the AINS right-handed or left-handed coordinate frame.
-
-
19. The pedometer navigator system of claim 17 wherein the computer means is coupled to receive and be responsive to
a series of time indexed inertial navigator velocity components {right arrow over (ν - )}SNVg,
a series of indexed body rates of the AINS {right arrow over (ω
)}Sb, and a data input characterizinga lever arm vector from the AINS center to the SBPMS origin {right arrow over (l)}S-Pb, the computer means and further comprising a computer program for calculating indexed values of an inertial navigation solution displacement Δ
{right arrow over (r)}SNV1-2g using the equation;
for a left foot inertial navigation solution displacement, and or the equation;
for a right foot inertial navigation solution displacement.
- )}SNVg,
-
20. The pedometer navigator system of claim 17 wherein the Kalman filter is coupled to receive and calculate the difference between an inertial navigation solution displacement and an SBPMS left and right relative displacement vector for each foot, the differences being calculated from the equations:
Specification