×

Satellite positioning system filter

  • US 5,590,043 A
  • Filed: 06/17/1993
  • Issued: 12/31/1996
  • Est. Priority Date: 06/17/1993
  • Status: Expired due to Term
0
Associated Cases
0
Associated Defendants
0
Product Citations
0
Petitions
49
Forward Citations
2
Assignments
First Claim
Patent Images

1. A method for filtering and using time-varying location coordinates Px,n, Py,n and Pz,n, determined by a Satellite Positioning System (SATPS), which operates in a static mode and in a dynamic mode, to determine the location of a mobile user of the SATPS and to reduce large discontinuities and control the rate at which a changing sequence of measurement errors can induce change in a sequence of the user'"'"'s location solution coordinates, the method comprising the steps of:

  • (1) Setting an integer n=1 initially, using a location determination system (LDS), having an LDS antenna and an LDS receiver/processor that is connected to the LDS antenna and that includes a computer, to receive a plurality of SATPS signals and to generate initial location coordinate fixes Px,n, Py,n, and Pz,n, for n =1, initializing a first selected group of variables, including a weight coefficient or gain factor α

    n (0≦

    α

    n ≦

    1.0;

    initially α

    1 =1.0), used for controlling the relative weighting of a new unfiltered location fix at a location fix time t=tn with a predicted value computed as the previous filtered position at time tn-1 and projected forward to the present time with the average velocity at time tn and tn-1 ;

    a steady state value α

    SS of the gain (0<

    α

    SS <

    1), used when the difference between the new location fix and predicted location fix, known as the prediction error, is small;

    a minimum value α

    min of the gain (0<

    α

    min <

    1), used when the prediction error is too high so that clamping of the gain is activated;

    a value α

    exp of the gain (0<

    α

    exp <

    1), to be used in a partial expansion mode of the system when the predicted location based on the average velocity may not allow an accurate estimate of the present location solution coordinates;

    a value α

    0 (0<

    α

    0 ≦

    1), used for the gain upon reset when large gaps in time occur between consecutive fix times tn, or when the prediction error is too large, or when a large improvement in location accuracy occurs between consecutive location fixes;

    a counting integer N (≧

    1), used for counting consecutive fixes in a static mode;

    a selected location error threshold PEthr ;

    a maximum allowable location error PEmax ;

    a maximum allowed time interval length Δ

    tmax during which no satellite signal measurements are received;

    a selected initial value of a variable, clamp-- active(n), which is >

    0 if clamping is active, <

    0 if the system is in a partial expansion mode, and =0 in a normal dynamic mode;

    a selected initial value of a variable, static-- count(n), used for counting the number of consecutive location fixes with velocity less than a threshold velocity Vh,thr that is required for entry into the static mode;

    a selected value β

    cl,n >

    1, used to control the incremental increase of the variable α

    n toward a steady state value α

    SS from a state in which clamping is present, where α

    SS =a selected positive value r30; and

    a selected value β

    SS (0<

    β

    SS <

    1), used to control a second approach of the variable α

    n from a reset value α

    (reset) toward a steady state value α

    SS in a normal dynamic mode of the SATPS, defining initial filtered location coordinates P.sup.Λ

    x,1 =Px,1, P.sup.Λ

    y,1 =Py,1 and P.sup.Λ

    z,1 =Pz,1, and proceeding to step (46);

    (2) for n≧

    2, using the LDS antenna and the LDS receiver/proceessor to receive a plurality Of SATPS signals and to generate new location coordinate fixes Px,n, Py,n, and Pz,n, new velocity coordinate fixes Vx,n, Vy,n, and Vz,n that are independent of any previous fix information received, a new time coordinate tn for the time of this location fix, and values for variables last α

    (n)=α

    n-1, last latency=Ln-1, static last fix(n)=1 if the preceding location fix (n-1) was in the static mode and =0 otherwise, last-- hvel(n)=Vh,n-1, last-- acc-- mode(n)=acc-- mode(n-1), last-- fix-- dim(n) =fix-- dim(n-1), and last-- Δ

    tm (n)=Δ

    tm (n-1) for variables computed for the preceding location fix;

    (3) computing the horizontal component Vh,n of velocity and the change in horizontal velocity Δ

    Vh,n =|Vh,n -Vh,n-1 | from the data obtained in step (2);

    (4) choosing a first set of values of filter variables including;

    req-- static-- count=a selected positive value r13, a selected value of static-- count required for entry into the static mode;

    a selected present value of do-- static(n), which=1 if the static mode is activated, and=0 otherwise;

    a selected present value of a variable fix-- dim(n)=2 or 3, according to whether the available SATPS geometry allows a 2-dimensional horizontal solution or a 3-dimensional location solution;

    a selected present clamping value α

    cl,n of the gain, with 0 <

    α

    cl,n <

    1, to be used when the prediction error is too high;

    a selected present value for a variable, new-- clamp(n), which=1 if clamping is activated at the nth fix, and=0 otherwise;

    a selected present value of a variable new-- fix-- set(n), which=1 if a dimension change (2-dimensional to 3-dimensional or 3-dimensional to 2-dimensional) or a satellite constellation change occurred at the nth fix, and=0 otherwise;

    a selected present value of a variable DSATPS-- ON(n), which=1 if differential satellite positioning system information is available at the time of the nth fix, and =0 otherwise;

    a selected present value of a variable SA-- OFF(n), which=1 if Selective Availability is inactivated at the time of the nth fix, and=0 otherwise;

    a selected present value for a variable acc-- mode(n), which is equal to the logical or Boolean sum;

    {DSATPS-- ON(n)) OR (SA-- OFF(n))};

    a selected present value of a variable, PDOP(n), representing the predicted dilution-of-precision determined from SATPS signals received from a selected constellation of SATPS satellites;

    a selected present value of a variable Δ

    tm (n)=max {tn -tml, tn -tm2, . . . , tn -tmM }, where tmi is the time of the most recent pseudorange measurement made from the i-th satellite that forms part of the satellite solution constellation at time tn, with tmi ≦

    tn (i=1, 2, . . . , M); and

    a selected value of a variable, B2G(n), that determines when the gain α

    n is to be reset if a selected condition is true;

    (5) determining if acc-- mode(n)=1 at the nth fix;

    when acc-- mode(n)=1, setting the following values for the indicated variables;

    Nmin (n)=r1, Nmax (n)=r2, Vh,thr (n)=r3 (m/sec), Δ

    Vh,thr (n)=r4 (m/sec), β

    cl,n =r5, and σ

    meas (n)=r6, σ

    pos (n)=PDOP(n)×

    σ

    meas (n);

    when acc-- mode(n)=0, setting the following values for the indicated variables;

    Nmin (n)=r7, Nmax (n)=r8, Vh,thr (n)=r9 (m/sec), Δ

    Vh,thr (n)=r10 (m/sec), β

    cl,n =r11, σ

    meas (n)=r12, and σ

    pos (n)=PDOP(n)×

    σ

    meas (n), where r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11 and r12 are selected positive values with r1 ≦

    r7, r2≦

    r8, r3≦

    r9, r4≦

    r10, r5≧

    r11>

    1, and r6--r12;

    (6) setting do static(n)=0;

    (7) testing if the horizontal velocity is below the static mode threshold by determining if the condition Vh,n <

    Vh,thr is true;

    when Vh,n <

    Vh,thr is true, incrementing static-- count(n) by 1 and proceeding to step (8);

    otherwise, continuing to step (8);

    (8) determining if static mode was active in the last fix by determining if {(static-- last-- fix(n)=1) AND (Δ

    Vh,n <

    Δ

    Vh,thr)} is true;

    when this condition is satisfied, setting do-- static(n)=1, setting clamp-- active(n)=0, and proceeding to step (13);

    otherwise, continuing to step (9);

    (9) determining if static mode was active in the last fix;

    if {static last fix(n)=1) AND (Δ

    Vh,n <

    Δ

    Vh,thr)} is true, setting static count(n) =0;

    if {(static last fix(n)=1) AND (Δ

    Vh,n ≧

    Δ

    Vh,thr)} is true, or if {static last fix(n)≠

    1) AND (static count(n)=req static count)} is true, setting do static(n)=1 and setting clamp active(n)=0;

    if {static last fix(n)≠

    1) AND (static count(n)≠

    req static count)} is true, continuing to step (10);

    (10) determining if do-- static(n)=1;

    when this condition is satisfied, setting last-- hvel(n+1)=Vh,n, setting Vx,n =Vy,n =Vz,n =0, replacing N by max(N, Nmin) and proceeding to step (12);

    otherwise proceeding to step (11);

    (11) determining if static-- last-- fix(n)=1;

    when this condition is not satisfied, proceeding to step (12);

    otherwise, setting new-- clamp(n)=1, setting end-- static-- count(n)=a selected positive integer r14, where end-- static-- count(n) is a countdown integer used for exiting from the static mode, and continuing to step (12);

    (12) setting Δ

    tn =tn-1 ;

    (13) determining if do-- static(n)=1;

    when this condition is satisfied, setting P.sup.Λ

    x,n- =P.sup.Λ

    x,n-1, P.sup.Λ

    y,n- =P.sup.Λ

    y,n-1 and P.sup.Λ

    z,n- =P.sup.Λ

    z,n-1 ;

    Otherwise, setting P.sup.Λ

    x,n- =P.sup.Λ

    x,n-1 +(Vx,n +Vx,n-1) Δ

    tn /2, setting P.sup.Λ

    y,n- =P.sup.Λ

    y,n-1 +(Vy,n +Vy,n-1)Δ

    tn /2 and setting P.sup.Λ

    z,n- =P.sup.Λ

    z,n-1 +(Vz,n +Vz,n-1) Δ

    Tn /2;

    (14) computing PEn =max {|Px,n -P.sup.Λ

    x,n- |, |Py,n -P.sup.Λ

    y,n- |, |Pz,n -P.sup.Λ

    z,n- |};

    (15) computing δ

    Vn =max {|Vx,n -Vx,n-1 |, |Vy,n -Vy,n-1 |, |Vz,n -Vz,n-1 |};

    (16) computing Δ

    tL (n)=max {Δ

    tn, Δ

    tm (n), Δ

    tm (n-1) };

    (17) determining if Δ

    tn ≧

    Δ

    tmax, where Δ

    tmax =a selected positive value r28;

    when this condition is satisfied, proceeding to step (25);

    otherwise, continuing to step (18);

    (18) determining if PEn ≧

    PEmax ;

    when this condition is satisfied, proceeding to step (25);

    otherwise, continuing to step (19);

    (19) determining if {(fix-- dim(n)=3) AND (first-- 3D-- fix(n)=1)} is true;

    when this condition is satisfied, proceeding to step (25);

    otherwise, continuing to step (20);

    (20) determining if B2G(n)≠

    1;

    when this condition is satisfied, proceeding to step (24);

    otherwise, continuing to step (21);

    (21) determining if {(first-- DSATPS-- fix(n)=1) AND (acc-- mode(n-1)=1)} is true;

    when this condition is satisfied, proceeding to step (25);

    otherwise, continuing to step (22);

    (22) determining if {(PDOP(n)<

    r15 PDOP(n-1)) AND (fix-- dim(n)=fix-- dim(n-1)) AND (PDOP(n-1)≧

    r16)} is true, where r15 and r16 are selected positive values;

    when this condition is satisfied, proceeding to step (25);

    otherwise, continuing to step (23);

    (23) determining if {(PEn ≧

    r17 σ

    pos (n)) AND (fix-- dim(n)=3) AND (fix-- dim(n-1)=2)} is true, where r17 is a selected positive value;

    when this condition is satisfied, continuing to step (25);

    otherwise, continuing to step (24);

    (24) setting reset-- filter(n)=0 and proceeding to step (26);

    (25) setting reset-- filter(n)=1, and setting the gain α

    n equal to the value α

    0 ;

    (26) determining if {(clamp-- active(n)>

    0) AND (α

    SS PEn <

    PEthr /r18) AND (end-- static-- count(n)=0)} is true, where r18 is a selected positive value;

    when this condition is satisfied, setting clamp-- active(n)=0;

    otherwise, continuing to step (27);

    (27) determining if {(clamp-- active(n)<

    0) AND (PEn ≧

    r19 last-- latency)} is true, where r19 is a selected positive value;

    when this condition is satisfied, setting clamp-- active(n)=0;

    otherwise, continuing to step (28);

    (28) determining if PEn ≧

    PEthr ;

    if the condition is not met, proceeding to step (45);

    otherwise, continuing to step (29);

    (29) computing a latency variable Ln =δ

    Vn Δ

    tL (n)/2, to estimate the maximum location error that has occurred due to presence of non-zero acceleration;

    (30) replacing the latency Ln by min(Ln, r20), where r20 is a selected positive value;

    (31) determining if PEn <

    r21 Ln, where r21 is a selected positive value;

    when this condition is not satisfied, proceeding to step (33);

    otherwise, continuing to step (32);

    (32) determining if {{(acc-- mode(n)=0) AND (Ln ≧

    r22)} OR {acc-- mode(n)=1 AND Ln ≧

    r23 }}is true;

    when this condition is satisfied, setting clamp-- active(n)=-r24, setting last-- latency=Ln, and proceeding to step (35), where r22, r23 and r24 are selected positive values;

    otherwise, continuing to step (33);

    (33) determining if α

    n PEn <

    PEthr ;

    when this condition is not satisfied, continuing to step (35);

    otherwise, continuing to step (34);

    (34) determining if (clamp-- active(n)=0) OR {(new-- fix-- set(n)=1) AND (clamp-- active(n)>

    0)} is true;

    when this condition is satisfied, setting new-- clamp(n)=1;

    otherwise, continuing to step (35);

    (35) determining if reset-- filter(n)=1;

    when this condition is not satisfied, continuing to step (36);

    otherwise, setting α

    =α

    0, setting clamp-- active(n)=0, setting end-- static-- count(n)=0, setting N=Nmin, and proceeding to step (45);

    (36) determining if new-- clamp(n)=1;

    when this condition is not satisfied, proceeding to step (40);

    otherwise, continuing to step (37);

    (37) determining if do-- static(n)=1;

    when this condition is not satisfied, proceeding to step (39);

    otherwise, continuing to step (38);

    (38) setting N=[r25 PEn ]int, where r25 is a selected positive value, replacing N by min(N, Nmax), setting α

    n =1/N, setting clamp-- active(n)=0, and proceeding to step (45);

    (39) setting α

    cl,n =Kn /PEn, setting α

    n =max(α

    cl,n, α

    min), where α

    min is a selected small positive value r29, setting clamp-- active (n)=Rn, where Kn and Rn are selected positive numbers that may depend on n, and proceeding to step (45);

    (40) determining if clamp-- active(n)>

    0;

    when this condition is not satisfied, proceeding to step (43);

    otherwise, continuing to step (41);

    (41) setting α

    n =Jn, replacing α

    n by max(α

    n-1, α

    SS), and setting clamp-- active(n)=clamp-- active(n)-1, where Jn is a selected positive number that depends upon n;

    (42) determining if end-- static-- count(n)>

    0;

    when this condition is satisfied, setting end-- static-- count(n)=end-- static-- count(n)-1 and proceeding to step (45);

    otherwise, continuing to step (43);

    (43) determining if clamp-- active(n)<

    0;

    when this condition is not satisfied, continuing to step (44);

    otherwise, setting α

    n =α

    exp, where α

    exp is a selected positive value r31, setting clamp-- active(n)=clamp-- active(n)+1, and proceeding to step (45);

    (44) determining if do-- static(n)=1;

    when this condition is satisfied, setting N=N+1, replacing N by min(N, Nmax), setting α

    n =1/N, and continuing to step (45);

    otherwise, setting α

    n =β

    SS α

    n-1, where β

    SS is a selected positive value r32, replacing α

    n by max(α

    n, α

    SS), and continuing to step (45);

    (45) computing filtered location coordinates P.sup.Λ

    x,n, P.sup.Λ

    y,n and P.sup.Λ

    z,n by the relationsP.sup.Λ

    x,n =α

    n Px,n +(1-α

    n)P.sup.Λ

    x,n-,P.sup.Λ

    y,n =α

    n Py,n +(1-α

    n)P.sup.Λ

    y,n-,P.sup.Λ

    z,n =α

    n Pz,n +(1-α

    n)P.sup.Λ

    z,n- ;

    (46) setting last α

    =α

    n, last latency=Ln, last PDOP =PDOP(n), last-- hvel(n+1)=Vh,n, last-- acc-- mode(n+1)=acc-- mode(n), last-- fix-- dim(n+1)=fix-- dim(n) and last-- Δ

    tm (n+1)=Δ

    tm (n);

    (47) replacing integer n by n+1;

    (48) returning to step (2) at least once; and

    (49) causing the LDS receiver/processor to assist the mobile SATPS user in navigating from a first selected location to a second selected location by replacing the location coordinates (Px,n, Py,n, Pz,n) by the filtered location coordinates (P.sup.Λ

    x,n, P.sup.Λ

    y,n, P.sup.Λ

    z,n) and by displaying these filtered location coordinates for at least one mobile user location other than the first and second selected locations.

View all claims
  • 2 Assignments
Timeline View
Assignment View
    ×
    ×