Pedometer navigator system

• US 20020143491A1
• Filed: 07/13/2001
• Published: 10/03/2002
• Est. Priority Date: 08/18/2000
• Status: Active Grant

• Current Assignee: Applanix Corp. (Trimble Inc.)
• Original Assignee: Applanix Corp. (Trimble Inc.)
• Inventors: Scherzinger, Bruno M.
• Granted Patent: US 6,594,617 B2
• Time in Patent Office: Days
• 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 is carried by a pedestrian, the pedometer system providing present position data as the pedestrian walks across terrain to be surveyed. The body frame is coupled to the pedestrian. An Euler Angle Measuring Subsystem (EAMS) coupled to a body frame measures and outputs the Euler angles of a fixed coordinate system with respect to an earth fixed navigational coordinate system. A left and right sensor is mounted on the pedestrian'"'"'s respective left and right foot. An SBPMS (short baseline position measuring subsystem) is coupled to the body frame and has a position measuring coordinate system. The SBPMS measures and provides a respective left vector {right arrow over (ρ)}_left^p and a right vector {right arrow over (ρ)}_right^p characterizing the position of the left and right sensors with respect to the position measuring coordinate system origin. A computer and algorithm means responds to the Euler angles of the body frame coordinate system with respect to the earth fixed navigational coordinate system and the left vector and the right vector by determining an interval during which the left or right foot is stationary and for providing a transition vector referenced to the earth fixed navigational coordinate system characterizing the movement of the SBPMS origin during the interval the respective foot is stationary. 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, 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 angle values for the right-handed or left-handed coordinate frame with respect to the X-Y-Z or north-east-down axes of a 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 and program means responsive to the 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 and program 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 the azimuth rotational rate of the baseline vector ψ

<sub>Δ

ρ</sub>

given by ω

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

<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 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 or 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 cap 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 and program 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 and program means further comprises an algorithm for calculating the SBPMS current position {right arrow over (r)}^g(t_current) 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^p 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 the rotational rate of the baseline vector ω

<sub>Δ

ρ</sub>

for each indexed baseline vector'"'"'s north component and east component, Δ

{dot over (ρ

)}_E and Δ

{dot over (ρ

)}_N, 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)<br><br>\left[\mathrm{BS16}\right]<br><br>\left[\mathrm{BS17}\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 or 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 Δ

{right arrow over (ρ

)}_1-2^g=C_p^g(t₂){right arrow over (ρ

)}_left^p(t₂)−

C_p^g(t₁){right arrow over (ρ

)}_left^p(t₁) and

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

{right arrow over (ρ

)}_2-3^g from Δ

{right arrow over (ρ

)}_2-3^g=C_p^g(t₃){right arrow over (ρ

)}_right^p(t₃)−

C_p^g(t₂){right arrow over (ρ

)}_right^p(t₂), and

to calculate an SBPMS position displacement vector Δ

{right arrow over (ρ

)}_1-3^g from Δ

{right arrow over (ρ

)}_1-3^g=Δ

{right arrow over (ρ

)}_1-2^g+Δ

{right arrow over (ρ

)}_2-3^g and where index times t₁ and t₂ for the SBPMS left relative displacement vector are respectively marked at the beginning and end of each left stationary signal and where the times t₂ and t₃ for the SBPMS right relative displacement vector are respectively marked at the beginning and end of each right stationary signal,

calculating 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 a SBPMS starting position {right arrow over (r)}^g_start, the SBPMS current position {right arrow over (r)}^g(t_curent) 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>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{\rightharpoonup <br><br>}{\mathrm{\rho <br><br>}}}_{1-3}^g<br><br>\left(t_i\right).$

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 values for the right-handed or left-handed coordinate frame with respect to the X-Y-Z or 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 (ρ

)}_l^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,

a computer means for executing a program responsive to the Euler angle values and the 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 and program means is coupled to receive and is responsive to time indexed inertial navigator velocity NED components {right arrow over (ν

)}_SNV^g, 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 and program means and further comprising a computer program for

calculating indexed values of the 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{\to <br><br>}{r}}_{\mathrm{SNV1–<br><br>2}}^g={\int <br><br>}_{t_1}^{t_2}<br><br>\left({\stackrel{\to <br><br>}{v}}_{\mathrm{SNV}}^g+C_b^g<br><br>\left({\stackrel{\to <br><br>}{\mathrm{\omega <br><br>}}}_S^b\times <br><br>{\stackrel{\to <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{\to <br><br>}{r}}_{\mathrm{SNV2–<br><br>3}}^g={\int <br><br>}_{\mathrm{t2}}^{\mathrm{t3}}<br><br>\left({\stackrel{\to <br><br>}{v}}_{\mathrm{SNV}}^g+C_b^g<br><br>\left({\stackrel{\to <br><br>}{\mathrm{\omega <br><br>}}}_S^b\times <br><br>{\stackrel{\to <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 the inertial navigator velocity NED components {right arrow over (ω

)}_SNV^g the IMU angular rate in the IMU body frame, {right arrow over (l)}_S-P^b the lever arm vector from the IMU inertial center to the SBPMS measurement origin.

9. The pedometer navigator system of claim 8 wherein the Kalman filter is coupled to receive and calculate the difference between the output the inertial navigation solution displacement and the SBPMS left or right relative displacement vectors for each foot, the differences being calculated from the equations:

{right arrow over (z)}_SNV-PP=Δ

{right arrow over (r)}_SNV1-2^g−

Δ

{right arrow over (ρ

)}_1-2^g for the left foot and {right arrow over (z)}_SNV-PP=Δ

{right arrow over (r)}_SNV2-3^g−

Δ

{right arrow over (ρ

)}_2-3^g for the right foot;

whereby,

the differences make the position, velocity and attitude errors in the inertial navigator and precise pedometer observable to the Kalman filter.

10. The pedometer navigator system of claim 7 wherein the computer and program 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 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^p 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 the 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 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)}foot_i^g(t) from {right arrow over (r)}_footi^g(t+Δ

t),

calculating the absolute value of the vector difference, and generates a foot stationary signal for each foot subject to the condition;

|{right arrow over (r)}_footi^g(t+Δ

t)−

{right arrow over (r)}_footi^g(t)|<

Δ

r_stationary for a predetermined time interval.

11. The pedometer navigator system of claim 7 wherein the computer and program 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 Euler angle values, the position 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 the absolute (right or left) indexed foot position {right arrow over (r)}_footi^g in geographic coordinates using the equation {right arrow over (r)}_footi^g={right arrow over (r)}_SBPMS^g+C_p^g{right arrow over (ρ

)}_l^p where C_p^g is a Direction Cosine Matrix (DCM) from the INS roll, pitch and heading solution that transforms from the p-frame to the g-frame, the indexed foot detection algorithm providing a signal indicating that the indexed foot is stationary on the condition that the difference between successive values of {right arrow over (r)}_footi^g remain below a predetermined threshold value for a time interval exceeding a predetermined time limit.

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 |{right arrow over (r)}_footi^g(t+Δ

t)−

{right arrow over (r)}_footi^g(t)|<

Δ

r_stationary for a time interval of at least one second.

14. The pedometer navigator system of claim 7 wherein the computer and program means further comprises an algorithm for calculating a right foot stationary signal and

a left stationary foot 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^g and right time indexed relative position vector {right arrow over (ρ

)}_right^g in fixed navigation coordinate frame values, p2 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 the 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 the 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 and program 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 Δ

{right arrow over (ρ

)}_1-2^g=C_p^g(t₂){right arrow over (ρ

)}_left^p(t₂)−

C_p^g(t₁){right arrow over (ρ

)}_left^p(t₁) and

using each right stationary signal to mark times t₂ and t₃ and to contemporaneously calculate an SBPMS right relative displacement vector Δ

{right arrow over (ρ

)}_2-3^g from Δ

{right arrow over (ρ

)}_2-3^g=C_p^g(t₃){right arrow over (ρ

)}_right^p(t₃)−

C_p^g(t₂){right arrow over (ρ

)}_right^p(t₂) and

calculating the SBPMS position displacement vector Δ

{right arrow over (ρ

)}_1-3^g from Δ

{right arrow over (ρ

)}_1-3^g=Δ

{right arrow over (ρ

)}_1-2^g+Δ

{right arrow over (ρ

)}_2-3^g and

where index times t₁ and t₂ are respectively marked at the beginning and end of each left stationary signal and where the times t₂ and t₃ are respectively marked at the beginning and end of each right stationary signal.

16. The pedometer navigator system of claim 14 wherein the computer and program 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 the 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 (ρ

)}_t^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 generates a foot stationary signal for each foot subject to the condition;

|{right arrow over (r)}_footi^g(t+Δ

t)−

{right arrow over (r)}_footi^g(t)|<

Δ

r_moving for a predetermined time interval.

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 the X-Y-Z or 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 an 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 and program means is coupled to receive and is responsive to time indexed inertial navigator velocity NED components {right arrow over (ν

)}_SNV, 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 and program means and further comprising a computer program for

calculating indexed values of the 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{\to <br><br>}{r}}_{\mathrm{SNV1–<br><br>2}}^g={\int <br><br>}_{t_1}^{t_2}<br><br>\left({\stackrel{\to <br><br>}{v}}_{\mathrm{SNV}}^g+C_b^g<br><br>\left({\stackrel{\to <br><br>}{\mathrm{\omega <br><br>}}}_S^b\times <br><br>{\stackrel{\to <br><br>}{l}}_{\mathrm{S–<br><br>P}}^b\right)\right)<br><br>\text{\hspace{1em}<br><br>}<br><br><br><br>t& \left(8<br><br>a\right)\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{\to <br><br>}{r}}_{\mathrm{SNV2–<br><br>3}}^g={\int <br><br>}_{\mathrm{t2}}^{\mathrm{t3}}<br><br>\left({\stackrel{\to <br><br>}{v}}_{\mathrm{SNV}}^g+C_b^g<br><br>\left({\stackrel{\to <br><br>}{\mathrm{\omega <br><br>}}}_S^b\times <br><br>{\stackrel{\to <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 wherein {right arrow over (ν

)}_SNV^g the inertial navigator velocity NED components {right arrow over (ω

)}_S^b the IMU angular rate in the IMU body frame, {right arrow over (l)}_S-P^b the lever arm vector from the IMU inertial center to the SBPMS measurement origin.

20. The pedometer navigator system of claim 17 wherein the Kalman filter is coupled to receive and calculate the difference between the output the inertial navigation solution displacement and the SBPMS left and right relative displacement vectors for each foot, the differences being calculated from the equations:

{right arrow over (z)}_SNV-PP=Δ

{right arrow over (r)}_SNV1-2^g−

Δ

{right arrow over (ρ

)}_1-2^g for the left foot and

{right arrow over (z)}_SNV-PP=Δ

{right arrow over (r)}_SNV2-3^g−

Δ

{right arrow over (ρ

)}_2-3^g for the right foot;

whereby,

the differences make the position, velocity and attitude errors in the inertial navigator and precise pedometer observable to the Kalman filter.

17. A pedometer navigator system carried by a pedestrian, comprising:

an Aided Inertial Navigation System (AINS) providing roll, pitch and azimuth 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,

the SBPMS coordinate frame being coupled to the AINS,

a computer means for executing a program responsive to the Euler angle values and the 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.