CN108088443A - A kind of positioning and directing device rate compensation method - Google Patents

A kind of positioning and directing device rate compensation method Download PDF

Info

Publication number
CN108088443A
CN108088443A CN201611037069.7A CN201611037069A CN108088443A CN 108088443 A CN108088443 A CN 108088443A CN 201611037069 A CN201611037069 A CN 201611037069A CN 108088443 A CN108088443 A CN 108088443A
Authority
CN
China
Prior art keywords
mrow
mtd
msub
msubsup
mtr
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201611037069.7A
Other languages
Chinese (zh)
Other versions
CN108088443B (en
Inventor
马涛
朱红
李永峰
石志兴
王根
郭元江
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Automation Control Equipment Institute BACEI
Original Assignee
Beijing Automation Control Equipment Institute BACEI
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Automation Control Equipment Institute BACEI filed Critical Beijing Automation Control Equipment Institute BACEI
Priority to CN201611037069.7A priority Critical patent/CN108088443B/en
Publication of CN108088443A publication Critical patent/CN108088443A/en
Application granted granted Critical
Publication of CN108088443B publication Critical patent/CN108088443B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
  • Navigation (AREA)

Abstract

The invention belongs to vehicle positioning and navigation technical fields, and in particular to a kind of positioning and directing device rate compensation method.The speed compensation method of the present invention, comprises the following steps：Establish Kalman filter model；System initialization；It is calculated to horizontal displacement components；East orientation and north orientation displacement component calculate；Velocity equivalent calculates, and updates Kalman filter model information；Kalman filtering calculating is carried out to system；Inertial navigation posture battle array, speed, site error are modified, odometer calibration factor and installation error are modified.The present invention needs to solve the accumulation that existing positioning and directing equipment is present with positioning and directing error during course maneuvering, the technical issues of precision of final influence navigator fix, side velocity is considered in calculating speed measurement information, effective compensation has been carried out to it, be conducive to reduce the positioning and directing error accumulation occurred during course maneuvering, positioning and directing equipment performance is improved, ensure that the precision of navigation.

Description

A kind of positioning and directing device rate compensation method
Technical field
The invention belongs to vehicle positioning and navigation technical fields, and in particular to a kind of positioning and directing device rate compensation method.
Background technology
It is a kind of very logical using inertial navigation and odometer composition positioning and directing equipment in terrestrial vehicle location navigation Means can obtain more satisfied performance.Compared to the integrated navigation system using satellite navigation composition, have entirely certainly It is main, it is round-the-clock, the advantages of interference from external information.While extensive use, to positioning and directing equipment performance requirement also with Promotion.
In existing positioning and orienting method, calculating is generally filtered using odometer velocity information and inertial navigation information, it is compatible Zero-velocity curve technology, to obtain higher positioning and directing precision.But in fact, often it is present with during course maneuvering Unexpected positioning and directing error, positioning and directing error can be accumulated gradually, the final precision for influencing navigator fix.
The content of the invention
The technical problem to be solved in the invention is：In existing positioning and orienting method, positioning and directing equipment is in course maneuvering It is present with the accumulation of positioning and directing error in the process, the final precision for influencing navigator fix.
It is described that technical scheme is as follows：
A kind of positioning and directing device rate compensation method, comprises the following steps：
Step 1 establishes Kalman filter model
It is as follows to define coordinate system：
N systems：Navigational coordinate system oxnynzn, for northeast day geographic coordinate system, xnAxis is directed toward east, ynAxis is directed toward north, znAxis is directed toward My god；
B systems：Carrier coordinate system oxbybzb, for right front upper right coordinate system, xbAxis is directed toward the right of carrier, ybAxis is directed toward carrier Front, zbAxis is directed toward the top of carrier；
The state vector X is taken to be
X=[δ Ve,δVn,δVu,φe,φn,φu,δλ,δL,δh,▽x,▽y,▽z,εx,εy,εz,φax,δKD,φaz,Δ t,Rx,Ry,Rz]T
Wherein,
[δVe,δVn,δVu] for velocity error vector, δ VeFor east orientation speed error, δ VnFor north orientation speed error, δ VuFor day To velocity error；
[φe,φn,φu] for attitude error vector, φeFor east orientation attitude error angle, φnFor north orientation attitude error angle, φu It is day to attitude error angle；
[δ λ, δ L, δ h] is site error vector, and δ λ are longitude error, δ L are latitude error, δ h are height error；
[▽x,▽y,▽z] for accelerometer bias vector, ▽xFor xbAxis accelerometer zero bias amount, ▽yFor ybAxle acceleration Count zero bias amount, ▽zFor zbAxis accelerometer zero bias amount；
[εx,εy,εz] for gyroscopic drift vector, εxFor xbAxis gyroscopic drift amount, εyFor ybAxis gyroscopic drift amount, εzFor zbAxis Gyroscopic drift amount；
[φax,δKD,φaz] for odometer error vector, φaxFor xbAxle misalignment, δ KDIt is missed for odometer calibration factor Difference, φazFor zbAxle misalignment；
Δt：For the time delay between inertial navigation/odometer speed；
[Rx,Ry,Rz] for lever arm vector, RxFor xbDirection of principal axis lever arm, RyFor ybDirection of principal axis lever arm, RzFor zbDirection of principal axis lever arm；
State equation is：
Wherein, G is system noise matrix, and W is system noise, and A is systematic observation matrix,
For from carrier coordinate system oxbybzbTo navigational coordinate system oxnynznTransformation matrix of coordinates, pass through navigate clearing It obtains；
ωieFor earth rotation angular speed, RMAnd RNRespectively earth meridian circle and prime vertical radius, L are latitude, VuFor day To speed, VnFor north orientation speed, VeFor east orientation speed, fe、fnAnd fuRespectively northeast day to equivalent acceleration measuring magnitude
Measurement equation is：
Z=HX+V
Wherein, Z is measurement, and H is measurement matrix, and V is measurement noise；
Respectively velocity equivalent east orientation, north orientation and day to component；
For transformation matrix of coordinatesThe i-th row jth column element, i=1,2,3；J=1,2,3；
VDFor odometer output equivalent speed；
For gyro to measure value vector；
The respectively first derivative of northeast sky orientation speed；
Step 2, system initialization
Inertial navigation system is aligned, and starts inertial navigation resolving, obtains attitude angle [θ, γ, ψ]T, speed[λ, φ, h]T, while displacement S is obtained by odometer；
Δ S=KD·NpluseFor scalar, KDFor odometer calibration factor, NplusePulse is exported for odometer；
The alignment methods use static-base alignment or moving alignment method；
Step 3, day are calculated to horizontal displacement components
The transformation matrix of coordinates obtained according to step 2 navigation calculationThe day of odometer displacement is calculated to component and level Component；
For 3 × 3 matrixes, then the day of odometer output displacement is to component：
Horizontal component is：
WhereinForThe 3rd row the 2nd row element；
Step 4, east orientation and north orientation displacement component calculate
The speed obtained according to step 2 navigation calculationCalculate east component and the north of odometer displacement To component；Specific method is as follows：
Wherein,For the east component of journey meter displacement,For the north component of journey meter displacement；
Step 5, velocity equivalent calculate, and update Kalman filter model information
The velocity equivalent in filtering cycle is calculated, specific formula is as follows：
Wherein,For velocity equivalent, Δ Sn(t) it is Δ SnIn the value of moment t,TeFor Filtering cycle, TnFor the navigation calculation cycle；
Sytem matrix A, observing matrix H are updated, calculates measurement Z；
Step 6 carries out Kalman filtering calculating to system；
Step 7 is modified inertial navigation posture battle array, speed, site error, to odometer calibration factor and installation error into Row is corrected；
Wherein, attitude error is φ=[X (3) X (4) X (5)]T,
Then inertial navigation posture battle array error correction formula isI is unit matrix；
Inertial navigation velocity error is δ V=[X (0) X (1) X (2)]T,
Then velocity error modification method is Vn=Vn-δV；
Inertial navigation site error modification method is
λ=λ-X (6)
L=L-X (7)
H=h-X (8)
Odometer scale coefficient error is Kd=X (15),
Then odometer scale coefficient error modification method is KODO=KODO×(1+Kd)；
Corresponding quantity of state should be set to zero after each correction amount use.
Beneficial effects of the present invention are：
The method of the present invention, vehicle caused by having been taken into full account in calculating speed measurement information during course maneuvering Side velocity, effective compensation has been carried out to it, has been conducive to reduce positioning and directing equipment and occurs during course maneuvering Positioning and directing error accumulation improves positioning and directing equipment performance, ensure that the precision of navigation.
Specific embodiment
It is as follows to define coordinate system：
N systems：Navigational coordinate system oxnynzn, for northeast day geographic coordinate system, xnAxis is directed toward east, ynAxis is directed toward north, znAxis is directed toward My god；
B systems：Carrier coordinate system oxbybzb, for right front upper right coordinate system, xbAxis is directed toward the right of carrier, ybAxis is directed toward carrier Front, zbAxis is directed toward the top of carrier.
The method of the present invention specifically includes following steps：
Step 1 establishes Kalman filter model
The state vector X is taken to be
X=[δ Ve,δVn,δVu,φe,φn,φu,δλ,δL,δh,▽x,▽y,▽z,εx,εy,εz,φax,δKD,φaz,Δ t,Rx,Ry,Rz]T
Wherein,
[δVe,δVn,δVu] for velocity error vector, δ VeFor east orientation speed error, δ VnFor north orientation speed error, δ VuFor day To velocity error；
[φe,φn,φu] for attitude error vector, φeFor east orientation attitude error angle, φnFor north orientation attitude error angle, φu It is day to attitude error angle；
[δ λ, δ L, δ h] is site error vector, and δ λ are longitude error, δ L are latitude error, δ h are height error；
[▽x,▽y,▽z] for accelerometer bias vector, ▽xFor xbAxis accelerometer zero bias amount, ▽yFor ybAxle acceleration Count zero bias amount, ▽zFor zbAxis accelerometer zero bias amount；
[εx,εy,εz] for gyroscopic drift vector, εxFor xbAxis gyroscopic drift amount, εyFor ybAxis gyroscopic drift amount, εzFor zbAxis Gyroscopic drift amount；
[φax,δKD,φaz] for odometer error vector, φaxFor xbAxle misalignment, δ KDIt is missed for odometer calibration factor Difference, φazFor zbAxle misalignment；
Δt：For the time delay between inertial navigation/odometer speed；
[Rx,Ry,Rz] for lever arm vector, RxFor xbDirection of principal axis lever arm, RyFor ybDirection of principal axis lever arm, RzFor zbDirection of principal axis lever arm.
State equation is：
Wherein, G is system noise matrix, and W is system noise, and A is systematic observation matrix,
For from carrier coordinate system oxbybzbTo navigational coordinate system oxnynznTransformation matrix of coordinates, by navigation settle accounts It arrives.
ωieFor earth rotation angular speed, RMAnd RNRespectively earth meridian circle and prime vertical radius, L are latitude, VuFor day To speed, VnFor north orientation speed, VeFor east orientation speed, fe、fnAnd fuRespectively northeast day to equivalent acceleration measuring magnitude
Measurement equation is：
Z=HX+V
Wherein, Z is measurement, and H is measurement matrix, and V is measurement noise；
Respectively velocity equivalent east orientation, north orientation and day to component；
For transformation matrix of coordinatesThe i-th row jth column element, i=1,2,3；J=1,2,3.
VDFor odometer output equivalent speed；
For gyro to measure value vector；
The respectively first derivative of northeast sky orientation speed；
Step 2, system initialization
Inertial navigation system is aligned, and starts inertial navigation resolving, obtains attitude angle [θ, γ, ψ]T, speed [λ, φ, h]T, while displacement S is obtained by odometer.
Δ S=KD·NpluseFor scalar, KDFor odometer calibration factor, NplusePulse is exported for odometer.
The alignment methods use static-base alignment or moving alignment method.
Step 3, day are calculated to horizontal displacement components
The transformation matrix of coordinates obtained according to step 2 navigation calculationThe day of odometer displacement is calculated to component and level Component.
For 3 × 3 matrixes, then the day of odometer output displacement is to component：
Horizontal component is：
WhereinForThe 3rd row the 2nd row element.
Step 4, east orientation and north orientation displacement component calculate
The speed obtained according to step 2 navigation calculationCalculate east component and the north of odometer displacement To component.Specific method is as follows：
Wherein,For the east component of journey meter displacement,For the north component of journey meter displacement.
Step 5, velocity equivalent calculate, and update Kalman filter model information
The velocity equivalent in filtering cycle is calculated, specific formula is as follows：
Wherein,For velocity equivalent, Δ Sn(t) it is Δ SnIn the value of moment t,TeFor Filtering cycle, TnFor the navigation calculation cycle.
Sytem matrix A, observing matrix H are updated, calculates measurement Z.
Step 6 carries out Kalman filtering calculating to system.
Step 7 is modified inertial navigation posture battle array, speed, site error, to odometer calibration factor and installation error into Row is corrected.
Wherein, attitude error is φ=[X (3) X (4) X (5)]T,
Then inertial navigation posture battle array error correction formula isI is unit matrix.
Inertial navigation velocity error is δ V=[X (0) X (1) X (2)]T,
Then velocity error modification method is Vn=Vn-δV。
Inertial navigation site error modification method is
λ=λ-X (6)
L=L-X (7)
H=h-X (8)
Odometer scale coefficient error is Kd=X (15),
Then odometer scale coefficient error modification method is KODO=KODO×(1+Kd)。
Corresponding quantity of state should be set to zero after each correction amount use.

Claims (1)

1. a kind of positioning and directing device rate compensation method, it is characterised in that：Comprise the following steps：
Step 1 establishes Kalman filter model
It is as follows to define coordinate system：
N systems：Navigational coordinate system oxnynzn, for northeast day geographic coordinate system, xnAxis is directed toward east, ynAxis is directed toward north, znAxis is directed toward day；
B systems：Carrier coordinate system oxbybzb, for right front upper right coordinate system, xbAxis is directed toward the right of carrier, ybBefore axis is directed toward carrier Side, zbAxis is directed toward the top of carrier；
The state vector X is taken to be
X=[δ Ve,δVn,δVu,φe,φn,φu,δλ,δL,δh,▽x,▽y,▽z,εx,εy,εz,φax,δKD,φaz,Δt,Rx, Ry,Rz]T
Wherein,
[δVe,δVn,δVu] for velocity error vector, δ VeFor east orientation speed error, δ VnFor north orientation speed error, δ VuIt is day to speed Spend error；
[φe,φn,φu] for attitude error vector, φeFor east orientation attitude error angle, φnFor north orientation attitude error angle, φuFor day To attitude error angle；
[δ λ, δ L, δ h] is site error vector, and δ λ are longitude error, δ L are latitude error, δ h are height error；
[▽x,▽y,▽z] for accelerometer bias vector, ▽xFor xbAxis accelerometer zero bias amount, ▽yFor ybAxis accelerometer zero Deviator, ▽zFor zbAxis accelerometer zero bias amount；
[εx,εy,εz] for gyroscopic drift vector, εxFor xbAxis gyroscopic drift amount, εyFor ybAxis gyroscopic drift amount, εzFor zbAxis gyro Drift value；
[φax,δKD,φaz] for odometer error vector, φaxFor xbAxle misalignment, δ KDFor odometer scale coefficient error, φazFor zbAxle misalignment；
Δt：For the time delay between inertial navigation/odometer speed；
[Rx,Ry,Rz] for lever arm vector, RxFor xbDirection of principal axis lever arm, RyFor ybDirection of principal axis lever arm, RzFor zbDirection of principal axis lever arm；
State equation is：
<mrow> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mi>A</mi> <mi>X</mi> <mo>+</mo> <mi>G</mi> <mi>W</mi> </mrow>
Wherein, G is system noise matrix, and W is system noise, and A is systematic observation matrix,
<mrow> <mi>A</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>A</mi> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mi>A</mi> <mn>2</mn> </msub> </mtd> <mtd> <msub> <mi>A</mi> <mn>3</mn> </msub> </mtd> <mtd> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>7</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>A</mi> <mn>4</mn> </msub> </mtd> <mtd> <msub> <mi>A</mi> <mn>5</mn> </msub> </mtd> <mtd> <msub> <mi>A</mi> <mn>6</mn> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>7</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>A</mi> <mn>7</mn> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>A</mi> <mn>8</mn> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>7</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mrow></mrow> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>13</mn> <mo>&amp;times;</mo> <mn>22</mn> </mrow> </msub> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow></mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>1</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>n</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>-</mo> <msub> <mi>V</mi> <mi>u</mi> </msub> <mo>)</mo> <mo>/</mo> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mo>+</mo> <mn>2</mn> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>V</mi> <mi>E</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mo>-</mo> <mn>2</mn> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mo>(</mo> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> <mo>)</mo> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>V</mi> <mi>u</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mo>/</mo> <mo>(</mo> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> <mo>)</mo> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msub> <mi>A</mi> <mn>2</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>f</mi> <mi>u</mi> </msub> </mrow> </mtd> <mtd> <msub> <mi>f</mi> <mi>n</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mi>u</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>f</mi> <mi>e</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>f</mi> <mi>n</mi> </msub> </mrow> </mtd> <mtd> <msub> <mi>f</mi> <mi>e</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>3</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mn>2</mn> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>u</mi> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <msub> <mi>V</mi> <mi>n</mi> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <msub> <mi>V</mi> <mi>n</mi> </msub> <msup> <mi>sec</mi> <mn>2</mn> </msup> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <msub> <mi>V</mi> <mi>u</mi> </msub> <mo>-</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <msub> <mi>V</mi> <mi>n</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <msub> <mi>V&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>-</mo> <msubsup> <mi>V</mi> <mi>e</mi> <mn>2</mn> </msubsup> <msup> <mi>sec</mi> <mn>2</mn> </msup> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>e</mi> </msub> <msub> <mi>V</mi> <mi>u</mi> </msub> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msubsup> <mi>V</mi> <mi>e</mi> <mn>2</mn> </msubsup> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <msub> <mi>V</mi> <mi>e</mi> </msub> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>V</mi> <mi>n</mi> <mn>2</mn> </msubsup> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msubsup> <mi>V</mi> <mi>e</mi> <mn>2</mn> </msubsup> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>4</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>1</mn> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>1</mn> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>5</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>-</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> <mo>-</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>6</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>e</mi> </msub> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <msup> <mi>sec</mi> <mn>2</mn> </msup> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>7</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>sec</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mn>1</mn> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>8</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mi> </mi> <mi>sec</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>sec</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
For from carrier coordinate system oxbybzbTo navigational coordinate system oxnynznTransformation matrix of coordinates, by navigate clearing obtain；
ωieFor earth rotation angular speed, RMAnd RNRespectively earth meridian circle and prime vertical radius, L are latitude, VuIt is day to speed Degree, VnFor north orientation speed, VeFor east orientation speed, fe、fnAnd fuRespectively northeast day to equivalent acceleration measuring magnitude
Measurement equation is：
Z=HX+V
Wherein, Z is measurement, and H is measurement matrix, and V is measurement noise；
<mrow> <mi>Z</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;V</mi> <mi>e</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;V</mi> <mi>n</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;V</mi> <mi>u</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>-</mo> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>e</mi> </mrow> <mi>n</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>-</mo> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>V</mi> <mi>u</mi> <mi>n</mi> </msubsup> <mo>-</mo> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>u</mi> </mrow> <mi>n</mi> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <mi>H</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>1</mn> <mo>,</mo> <mn>3</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>1</mn> <mo>,</mo> <mn>1</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>e</mi> </msub> </mtd> <mtd> <mrow></mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>(</mo> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;times;</mo> <mo>)</mo> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>9</mn> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>2</mn> <mo>,</mo> <mn>3</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>2</mn> <mo>,</mo> <mn>2</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>2</mn> <mo>,</mo> <mn>1</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <mrow> <mo>(</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mo>&amp;times;</mo> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>,</mo> <mn>3</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>,</mo> <mn>2</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>,</mo> <mn>1</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>u</mi> </msub> </mtd> <mtd> <mrow></mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
Respectively velocity equivalent east orientation, north orientation and day to component；
For transformation matrix of coordinatesThe i-th row jth column element, i=1,2,3；J=1,2,3；
VDFor odometer output equivalent speed；
For gyro to measure value vector；
The respectively first derivative of northeast sky orientation speed；
Step 2, system initialization
Inertial navigation system is aligned, and starts inertial navigation resolving, obtains attitude angle [θ, γ, ψ]T, speed [λ, φ, h]T, while displacement S is obtained by odometer；
Δ S=KD·NpluseFor scalar, KDFor odometer calibration factor, NplusePulse is exported for odometer；
The alignment methods use static-base alignment or moving alignment method；
Step 3, day are calculated to horizontal displacement components
The transformation matrix of coordinates obtained according to step 2 navigation calculationThe day of odometer displacement is calculated to component and horizontal component；
For 3 × 3 matrixes, then the day of odometer output displacement is to component：
<mrow> <msubsup> <mi>&amp;Delta;S</mi> <mi>u</mi> <mi>n</mi> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>,</mo> <mn>2</mn> <mo>)</mo> </mrow> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>S</mi> <mo>,</mo> </mrow>
Horizontal component is：
<mrow> <msubsup> <mi>&amp;Delta;S</mi> <mrow> <mi>l</mi> <mi>e</mi> <mi>v</mi> <mi>e</mi> <mi>l</mi> </mrow> <mi>n</mi> </msubsup> <mo>=</mo> <msqrt> <mrow> <mn>1</mn> <mo>-</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>(</mo> <mrow> <mn>3</mn> <mo>,</mo> <mn>2</mn> </mrow> <mo>)</mo> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>S</mi> <mo>,</mo> </mrow>
WhereinForThe 3rd row the 2nd row element；
Step 4, east orientation and north orientation displacement component calculate
The speed obtained according to step 2 navigation calculationCalculate the east component of odometer displacement and north orientation point Amount；Specific method is as follows：
<mrow> <msubsup> <mi>&amp;Delta;S</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfrac> <msubsup> <mi>V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <msqrt> <mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mfrac> <mo>&amp;CenterDot;</mo> <msubsup> <mi>&amp;Delta;S</mi> <mrow> <mi>l</mi> <mi>e</mi> <mi>v</mi> <mi>e</mi> <mi>l</mi> </mrow> <mi>n</mi> </msubsup> </mrow>
<mrow> <msubsup> <mi>&amp;Delta;S</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfrac> <msubsup> <mi>V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <msqrt> <mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mfrac> <mo>&amp;CenterDot;</mo> <msubsup> <mi>&amp;Delta;S</mi> <mrow> <mi>l</mi> <mi>e</mi> <mi>v</mi> <mi>e</mi> <mi>l</mi> </mrow> <mi>n</mi> </msubsup> </mrow>
Wherein,For the east component of journey meter displacement,For the north component of journey meter displacement；
Step 5, velocity equivalent calculate, and update Kalman filter model information
The velocity equivalent in filtering cycle is calculated, specific formula is as follows：
<mrow> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> </mrow> <mi>n</mi> </msubsup> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>e</mi> </mrow> <mi>n</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>u</mi> </mrow> <mi>n</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>=</mo> <mfrac> <mrow> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>t</mi> <mo>=</mo> <msub> <mi>T</mi> <mi>n</mi> </msub> </mrow> <msub> <mi>T</mi> <mi>e</mi> </msub> </munderover> <msup> <mi>&amp;Delta;S</mi> <mi>n</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <msub> <mi>T</mi> <mi>e</mi> </msub> </mfrac> </mrow>
Wherein,For velocity equivalent, Δ Sn(t) it is Δ SnIn the value of moment t,TeFor filtering Cycle, TnFor the navigation calculation cycle；
Sytem matrix A, observing matrix H are updated, calculates measurement Z；
Step 6 carries out Kalman filtering calculating to system；
Step 7 is modified inertial navigation posture battle array, speed, site error, and odometer calibration factor and installation error are repaiied Just；
Wherein, attitude error is φ=[X (3) X (4) X (5)]T,
Then inertial navigation posture battle array error correction formula isI is unit matrix；
Inertial navigation velocity error is δ V=[X (0) X (1) X (2)]T,
Then velocity error modification method is Vn=Vn-δV；
Inertial navigation site error modification method is
λ=λ-X (6)
L=L-X (7)
H=h-X (8)
Odometer scale coefficient error is Kd=X (15),
Then odometer scale coefficient error modification method is KODO=KODO×(1+Kd)；
Corresponding quantity of state should be set to zero after each correction amount use.
CN201611037069.7A 2016-11-23 2016-11-23 Speed compensation method for positioning and orienting equipment Active CN108088443B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201611037069.7A CN108088443B (en) 2016-11-23 2016-11-23 Speed compensation method for positioning and orienting equipment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201611037069.7A CN108088443B (en) 2016-11-23 2016-11-23 Speed compensation method for positioning and orienting equipment

Publications (2)

Publication Number Publication Date
CN108088443A true CN108088443A (en) 2018-05-29
CN108088443B CN108088443B (en) 2021-06-08

Family

ID=62169960

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201611037069.7A Active CN108088443B (en) 2016-11-23 2016-11-23 Speed compensation method for positioning and orienting equipment

Country Status (1)

Country Link
CN (1) CN108088443B (en)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109059913A (en) * 2018-08-27 2018-12-21 立得空间信息技术股份有限公司 A kind of zero-lag integrated navigation initial method for onboard navigation system
CN109143304A (en) * 2018-09-30 2019-01-04 百度在线网络技术（北京）有限公司 Method and apparatus for determining automatic driving vehicle pose
CN109827572A (en) * 2019-03-12 2019-05-31 北京星网宇达科技股份有限公司 A kind of method and device of detection truck position prediction
CN109974697A (en) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 A kind of high-precision mapping method based on inertia system
CN110514221A (en) * 2019-08-13 2019-11-29 中国航空工业集团公司西安飞行自动控制研究所 A kind of mileage gauge initial parameter quick calculation method
CN110657788A (en) * 2018-06-29 2020-01-07 北京自动化控制设备研究所 Dynamic detection method for smoothness of crane track
CN110873563A (en) * 2018-08-30 2020-03-10 杭州海康机器人技术有限公司 Cloud deck attitude estimation method and device
CN111912405A (en) * 2019-05-10 2020-11-10 中国人民解放***箭军工程大学 Combined navigation method and system based on vehicle-mounted inertial measurement unit and Doppler radar
CN112284415A (en) * 2020-10-19 2021-01-29 株洲菲斯罗克光电技术有限公司 Odometer scale error calibration method, system and computer storage medium
CN115773751A (en) * 2023-02-13 2023-03-10 中国航空工业集团公司西安飞行自动控制研究所 Method for correcting alignment error caused by zero position of equivalent antenna direction adder

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101672650A (en) * 2009-09-29 2010-03-17 北京航空航天大学 Orienting and locating navigation system in circumstance of electromagnetic interference
CN103217157A (en) * 2012-01-18 2013-07-24 北京自动化控制设备研究所 Inertial navigation/mileometer autonomous integrated navigation method
CN104977004A (en) * 2015-07-13 2015-10-14 湖北航天技术研究院总体设计所 Method and system for integrated navigation of laser inertial measuring unit and odometer

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101672650A (en) * 2009-09-29 2010-03-17 北京航空航天大学 Orienting and locating navigation system in circumstance of electromagnetic interference
CN103217157A (en) * 2012-01-18 2013-07-24 北京自动化控制设备研究所 Inertial navigation/mileometer autonomous integrated navigation method
CN104977004A (en) * 2015-07-13 2015-10-14 湖北航天技术研究院总体设计所 Method and system for integrated navigation of laser inertial measuring unit and odometer

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
PENGXIANG YANG: ""Local Feedback Compensation Method for INS/GPS/OD Land Navigation System"", 《PROCEEDINGS OF THE 2008 IEEE INTERNATIONAL CONFERENCE ON INFORMATION AND AUTOMATION》 *
陈鸿跃等: ""一种里程计辅助车载捷联惯导行进间对准方法"", 《导弹与航天运载技术》 *

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110657788A (en) * 2018-06-29 2020-01-07 北京自动化控制设备研究所 Dynamic detection method for smoothness of crane track
CN109059913A (en) * 2018-08-27 2018-12-21 立得空间信息技术股份有限公司 A kind of zero-lag integrated navigation initial method for onboard navigation system
CN110873563A (en) * 2018-08-30 2020-03-10 杭州海康机器人技术有限公司 Cloud deck attitude estimation method and device
CN109143304A (en) * 2018-09-30 2019-01-04 百度在线网络技术（北京）有限公司 Method and apparatus for determining automatic driving vehicle pose
CN109143304B (en) * 2018-09-30 2020-12-29 百度在线网络技术（北京）有限公司 Method and device for determining pose of unmanned vehicle
CN109827572A (en) * 2019-03-12 2019-05-31 北京星网宇达科技股份有限公司 A kind of method and device of detection truck position prediction
CN109974697A (en) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 A kind of high-precision mapping method based on inertia system
CN109974697B (en) * 2019-03-21 2022-07-26 中国船舶重工集团公司第七0七研究所 High-precision mapping method based on inertial system
CN111912405A (en) * 2019-05-10 2020-11-10 中国人民解放***箭军工程大学 Combined navigation method and system based on vehicle-mounted inertial measurement unit and Doppler radar
CN110514221A (en) * 2019-08-13 2019-11-29 中国航空工业集团公司西安飞行自动控制研究所 A kind of mileage gauge initial parameter quick calculation method
CN110514221B (en) * 2019-08-13 2023-03-14 中国航空工业集团公司西安飞行自动控制研究所 Rapid calculation method for initial parameters of mileage instrument
CN112284415A (en) * 2020-10-19 2021-01-29 株洲菲斯罗克光电技术有限公司 Odometer scale error calibration method, system and computer storage medium
CN115773751A (en) * 2023-02-13 2023-03-10 中国航空工业集团公司西安飞行自动控制研究所 Method for correcting alignment error caused by zero position of equivalent antenna direction adder

Also Published As

Publication number Publication date
CN108088443B (en) 2021-06-08

Similar Documents

Publication Publication Date Title
CN108088443A (en) A kind of positioning and directing device rate compensation method
CN108051866B (en) Based on strap down inertial navigation/GPS combination subsidiary level angular movement isolation Gravimetric Method
CN101893445B (en) Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition
CN108180925A (en) A kind of odometer assists vehicle-mounted dynamic alignment method
CN103245359B (en) A kind of inertial sensor fixed error real-time calibration method in inertial navigation system
CN103900565B (en) A kind of inertial navigation system attitude acquisition method based on differential GPS
CN105823481A (en) GNSS-INS vehicle attitude determination method based on single antenna
CN105698822B (en) Initial Alignment Method between autonomous type inertial navigation based on reversed Attitude Tracking is advanced
CN106507913B (en) Combined positioning method for pipeline mapping
CN104977004B (en) A kind of used group of laser and odometer Combinated navigation method and system
CN104697526A (en) Strapdown inertial navitation system and control method for agricultural machines
CN107270893A (en) Lever arm, time in-synchronization error estimation and the compensation method measured towards real estate
CN109974697A (en) A kind of high-precision mapping method based on inertia system
CN107588769A (en) A kind of vehicle-mounted inertial navigation, odometer and altimeter Combinated navigation method
CN109186597B (en) Positioning method of indoor wheeled robot based on double MEMS-IMU
CN102679978B (en) Initial alignment method of static base of rotary type strap-down inertial navigation system
CN102519485B (en) Gyro information-introduced double-position strapdown inertial navigation system initial alignment method
CN101949703A (en) Strapdown inertial/satellite combined navigation filtering method
CN103217174B (en) A kind of strapdown inertial navitation system (SINS) Initial Alignment Method based on low precision MEMS (micro electro mechanical system)
CN109612460B (en) Plumb line deviation measuring method based on static correction
CN111678514B (en) Vehicle-mounted autonomous navigation method based on carrier motion condition constraint and single-axis rotation modulation
CN104165638A (en) Multi-position self-calibration method for biaxial rotating inertial navigation system
CN102853837A (en) MIMU and GNSS information fusion method
CN105737842A (en) Vehicle-mounted autonomous navigation method based on rotary modulation and virtual odometer
CN109470276B (en) Odometer calibration method and device based on zero-speed correction

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant