Pedometer navigator system

• US 6,594,617 B2
• Filed: 07/13/2001
• Issued: 07/15/2003
• Est. Priority Date: 08/18/2000
• Status: Expired due to Term

• Current Assignee: Applanix Corp. (Trimble Inc.)
• Original Assignee: Applanix Corp. (Trimble Inc.)
• Inventors: Scherzinger, Bruno M.
• Primary Examiner(s): Shah, Kamini
• Assistant Examiner(s): Washburn, Douglas N
• Application Number:
  US09/905,015
• Publication Number:
  US 20020143491A1
• Time in Patent Office: 732 Days
• Field of Search: 364/450, 600/595, 701/1, 701/200, 701/207
• US Class Current: 702/160
• CPC Class Codes: G01C 21/188 for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
• CPC Class Codes: G01C 22/006 Pedometers

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 (ρ)}_left^p and a right vector {right arrow over (ρ)}_right^p 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.

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 (ρ

)}_left^p and right position vectors {right arrow over (ρ

)}_right^p 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.

2. The pedometer navigator system of claim 1 further comprising:

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 (ρ

)}_left^p and right position vector {right arrow over (ρ

)}_right^p by the respective time indexed DCM to obtain a respective time indexed left time indexed relative position vector {right arrow over (ρ

)}_left^g and right time indexed relative position vector {right arrow over (ρ

)}_right^g 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 ω

<sub>Δ

ρ</sub>

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, Δ

ρ

_N²+Δ

ρ

_E², and

calculating the value of the azimuth rotational rate of the time indexed baseline vector ω

<sub>Δ

ρ</sub>

from the following equation for each pair of indexed baseline vector'"'"'s north component and east component, ${\mathrm{\omega <br><br>}}_{\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\rho <br><br>}}=\frac{1}{\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_N^2+\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_E^2}<br><br>\left(\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_N<br><br>\mathrm{\Delta <br><br>}<br><br>{\stackrel{.}{\mathrm{\rho <br><br>}}}_E-\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_E<br><br>\mathrm{\Delta <br><br>}<br><br>{\stackrel{.}{\mathrm{\rho <br><br>}}}_N\right)$

 

the computer means being further characterized to provide

the 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 ω

<sub>Δ

ρ</sub>

is below a predetermined (and substantially zero) noise threshold, and provide

the 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 ω

<sub>Δ

ρ</sub>

exceeding the predetermined noise threshold and the sign of ω

<sub>Δ

ρ</sub>

being positive, i.e., ω

<sub>Δ

ρ</sub>

<

0, and provide

the 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 ω

<sub>Δ

ρ</sub>

exceeding the predetermined noise threshold and the sign of ω

<sub>Δ

ρ</sub>

being negative, i.e., ω

<sub>Δ

ρ</sub>

<

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 (t_current) by the steps of:

adding all SBPMS position displacement vectors Δ

{right arrow over (ρ

)}_1-3^g from a starting time to the current time,

summing the result of the previous step with the SBPMS starting position {right arrow over (r)}^g_start, the sum being characterized by the equation ${\stackrel{\rightharpoonup <br><br>}{r}}^g<br><br>\left(t_{\mathrm{current}}\right)={\stackrel{\rightharpoonup <br><br>}{r}}_{\mathrm{start}}^g+\underset{t_{\mathrm{start}}}{\overset{t_{\mathrm{current}}}{\sum <br><br>}}<br><br>\mathrm{\Delta <br><br>}<br><br>{\stackrel{\rightharpoonup <br><br>}{\mathrm{\rho <br><br>}}}_{1-3}^g<br><br>\left(t_i\right).$

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 (t_curret) 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 (ρ

)}_left^p and right position vector {right arrow over (ρ

)}_right^g by the respective DCM to obtain a respective left time indexed relative position vector {right arrow over (ρ

)}_left^g and right time indexed relative position vector {right arrow over (ρ

)}_right^g 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 ω

<sub>Δ

ρ</sub>

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, ${\mathrm{\omega <br><br>}}_{\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\rho <br><br>}}=\frac{1}{\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_N^2+\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_E^2}<br><br>\left(\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_N<br><br>\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{.}{\mathrm{\rho <br><br>}}}_E-\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_E<br><br>\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{.}{\mathrm{\rho <br><br>}}}_N\right)$

 

the computer and algorithm means being further characterized to provide

the 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 ω

<sub>Δ

ρ</sub>

is below a predetermined (and substantially zero) noise threshold, and provide

the 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 ω

<sub>Δ

ρ</sub>

exceeding the predetermined noise threshold and the sign of ω

<sub>Δ

ρ</sub>

being positive, i.e., ω

<sub>Δ

ρ</sub>

<

0, and provide

the 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 ω

<sub>Δ

ρ</sub>

exceeding the predetermined noise threshold and the sign of ω

<sub>Δ

ρ</sub>

being negative, i.e., ω

<sub>Δ

ρ</sub>

<

0 being negative,

the steps for calculating the SBPMS current position {right arrow over (r)}^g (t_current) further comprising the steps of;

using each left stationary signal to mark times t₁ and t₂ and contemporaneously calculate a left indexed SBPMS left relative displacement vector Δ

{right arrow over (ρ

)}_1-2^g 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 (ρ

)}_i^p, 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.

8. The pedometer navigator system of claim 7 wherein the computer means is coupled to receive and is responsive to a series of time indexed inertial navigator velocity North-east-down components {right arrow over (ν

)}_SNV^g, a series of indexed body rates of the AINS {right arrow over (ω

)}_S^b, as inputs from the AINS and a data input characterizing

a lever arm vector from the AINS center to the SBPMS origin {right arrow over (l)}_S-P^b, the computer means and further comprising;

a computer program for

calculating indexed values of an inertial navigation solution displacement Δ

{right arrow over (r)}_SNV1-2^g using the equation;

$\begin{array}{cc}\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{\rightharpoonup <br><br>}{r}}_{\mathrm{SNV1–<br><br>2}}^g={\int <br><br>}_{t_1}^{t_2}<br><br>\left({\stackrel{\rightharpoonup <br><br>}{v}}_{\mathrm{SNV}}^g+C_b^g<br><br>\left({\stackrel{\rightharpoonup <br><br>}{\mathrm{\omega <br><br>}}}_S^b\times <br><br>{\stackrel{\rightharpoonup <br><br>}{l}}_{\mathrm{S–<br><br>P}}^b\right)\right)<br><br>\text{\hspace{1em}<br><br>}<br><br><br><br>t& \text{(8a)}\end{array}$

 

for a left foot inertial navigation solution displacement, and or the equation;

$\begin{array}{cc}\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{\rightharpoonup <br><br>}{r}}_{\mathrm{SNV2–<br><br>3}}^g={\int <br><br>}_{\mathrm{t2}}^{\mathrm{t3}}<br><br>\left({\stackrel{\rightharpoonup <br><br>}{v}}_{\mathrm{SNV}}^g+C_b^g<br><br>\left({\stackrel{\rightharpoonup <br><br>}{\mathrm{\omega <br><br>}}}_S^b\times <br><br>{\stackrel{\rightharpoonup <br><br>}{l}}_{\mathrm{S–<br><br>P}}^b\right)\right)<br><br>\text{\hspace{1em}<br><br>}<br><br><br><br>t& \text{(8b)}\end{array}$

 

for a right foot inertial navigation solution

displacement wherein {right arrow over (ν

)}_SNV^g represents the series of time indexed inertial navigator velocity NED components, {right arrow over (ω

)}_S^b represents the series of body rates of the AINS and {right arrow over (l)}_S-P^b 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)}_footi^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 (ρ

)}_i^g by its respective DCM to obtain a respective time-indexed SBPMS relative position vector {right arrow over (ρ

)}_i^g in fixed navigation coordinate frame values at index time t,

fetching an indexed geographic position of the SBPMS, {right arrow over (r)}_SBPMS^g, from the AINS at index time t,

adding the indexed geographic position of the SBPMS, {right arrow over (r)}_SBPMS^g to the respective time indexed relative position vector {right arrow over (ρ

)}_i^g 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 (ρ

)}_i^p by its respective DCM to obtain a respective time indexed relative position vector {right arrow over (ρ

)}_i^g in fixed navigation coordinate frame values at index time t+Δ

t,

fetching the next indexed geographic position of the SBPMS, {right arrow over (r)}_SBPMS^g, from the AINS at index time t+Δ

t,

adding the indexed geographic position of the next SBPMS, {right arrow over (r)}_SBPMS^g to the respective next time indexed relative position vector {right arrow over (ρ

)}_i^g to obtain a value for {right arrow over (r)}_footi^g at time t+Δ

t,

subtracting the value of {right arrow over (r)}_footi^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 (ρ

)}_i^p, and

a computed value of the indexed geographic position of the SBPMS, {right arrow over (r)}_SBPMS^g, from the AINS by calculating

an absolute (right or left) indexed foot position {right arrow over (r)}_footi^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 (ρ

)}_left^p and or right position vector {right arrow over (ρ

)}_right^p by the respective DCM to obtain a respective left time indexed relative position vector {right arrow over (ρ

)}_left^p and right time indexed relative position vector {right arrow over (ρ

)}_right^p 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 the rotational rate of the baseline vector ω

<sub>Δ

ρ</sub>

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, Δ

ρ

_N²+Δ

ρ

_E², and

calculating the value of ω

<sub>Δ

ρ</sub>

from the following equation for each pair of indexed baseline vector'"'"'s north component and east component, ${\mathrm{\omega <br><br>}}_{\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\rho <br><br>}}=\frac{1}{\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_N^2+\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_E^2}<br><br>\left(\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_N<br><br>\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{.}{\mathrm{\rho <br><br>}}}_E-\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\rho <br><br>}}_E<br><br>\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{.}{\mathrm{\rho <br><br>}}}_N\right)$

 

the computer and algorithm means being further characterized to provide

a 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 ω

<sub>Δ

ρ</sub>

exceeding a predetermined noise threshold and the sign of ω

<sub>Δ

ρ</sub>

being positive, i.e., ω

<sub>Δ

ρ</sub>

<

0, and provide

a 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 ω

<sub>Δ

ρ</sub>

exceeding a predetermined noise threshold and the sign of ω

<sub>Δ

ρ</sub>

being negative, i.e., ω

<sub>Δ

ρ</sub>

<

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 t₁ and t₂ and contemporaneously calculate an SBPMS left relative displacement vector Δ

{right arrow over (ρ

)}_1-2^g 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)}_footi^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 (ρ

)}_i^p by its respective DCM to obtain a respective time indexed relative position vector {right arrow over (ρ

)}_i^g in fixed navigation coordinate frame values at index time t,

fetching an indexed geographic position of the SBPMS, {right arrow over (r)}_SBPMS^g, from the AINS at index time t,

adding the indexed geographic position of the SBPMS, {right arrow over (r)}_SBPMS^g to the respective time indexed relative position vector {right arrow over (ρ

)}_i^g 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 (ρ

)}_i^p by its respective DCM to obtain a respective time indexed relative position vector {right arrow over (ρ

)}_i^g in fixed navigation coordinate frame values at index time t+Δ

t,

fetching the next indexed geographic position of the SBPMS, {right arrow over (ρ

)}_SBPMS^g, from the AINS at index time t+Δ

t,

adding the indexed geographic position of the next SBPMS, {right arrow over (r)}_SBPMS^g to the respective next time indexed relative position vector {right arrow over (r)}_i^g to obtain a value for {right arrow over (r)}_footi^g at time t+Δ

t,

subtracting the value of {right arrow over (r)}_footi^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 (ρ

)}_i^p, 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.

18. The pedometer navigator system of claim 17 wherein Aided Inertial Navigation System (AINS) further comprises:

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 (ρ

)}_i^p, 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 (ν

)}_SNV^g,

a series of indexed body rates of the AINS {right arrow over (ω

)}_S^b, and a data input characterizing

a lever arm vector from the AINS center to the SBPMS origin {right arrow over (l)}_S-P^b, the computer means and further comprising a computer program for

calculating indexed values of an inertial navigation solution displacement Δ

{right arrow over (r)}_SNV1-2^g using the equation;

$\begin{array}{cc}\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{\rightharpoonup <br><br>}{r}}_{\mathrm{SNV1–<br><br>2}}^g={\int <br><br>}_{t_1}^{t_2}<br><br>\left({\stackrel{\rightharpoonup <br><br>}{v}}_{\mathrm{SNV}}^g+C_b^g<br><br>\left({\stackrel{\rightharpoonup <br><br>}{\mathrm{\omega <br><br>}}}_S^b\times <br><br>{\stackrel{\rightharpoonup <br><br>}{l}}_{\mathrm{S–<br><br>P}}^b\right)\right)<br><br>\text{\hspace{1em}<br><br>}<br><br><br><br>t& \text{(8a)}\end{array}$

 

for a left foot inertial navigation solution displacement, and or the equation;

$\begin{array}{cc}\mathrm{\Delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{\rightharpoonup <br><br>}{r}}_{\mathrm{SNV2–<br><br>3}}^g={\int <br><br>}_{\mathrm{t2}}^{\mathrm{t3}}<br><br>\left({\stackrel{\rightharpoonup <br><br>}{v}}_{\mathrm{SNV}}^g+C_b^g<br><br>\left({\stackrel{\rightharpoonup <br><br>}{\mathrm{\omega <br><br>}}}_S^b\times <br><br>{\stackrel{\rightharpoonup <br><br>}{l}}_{S-P}^b\right)\right)<br><br>\text{\hspace{1em}<br><br>}<br><br><br><br>t& \text{(8b)}\end{array}$

 

for a right foot inertial navigation solution displacement.

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: