CN107289932A - Single deck tape-recorder Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions - Google Patents
Single deck tape-recorder Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions Download PDFInfo
- Publication number
- CN107289932A CN107289932A CN201710505803.6A CN201710505803A CN107289932A CN 107289932 A CN107289932 A CN 107289932A CN 201710505803 A CN201710505803 A CN 201710505803A CN 107289932 A CN107289932 A CN 107289932A
- Authority
- CN
- China
- Prior art keywords
- mrow
- mtd
- msub
- mtr
- msubsup
- 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
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
- G01C21/165—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 combined with non-inertial navigation instruments
-
- 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
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/16—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using electromagnetic waves other than radio waves
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
- Electrochromic Elements, Electrophoresis, Or Variable Reflection Or Absorption Elements (AREA)
Abstract
The invention discloses a kind of single deck tape-recorder Kalman Filtering guider based on MEMS sensor and VLC positioning fusions and method, including MEMS sensor, INS modules, VLC locating modules, PDR locating modules and survey appearance positioning single Kalman filter module；The error equation based on INS inertial navigation mechanizations of the invention is as the system equation of fused filtering device, and observational equation includes VLC positioning information updates, PDR positioning information updates and magnetometer observed quantity and update.The posture that fused filtering device exports VLC receivers gives VLC locating modules, exports the posture of PDR equipment to PDR locating modules to correct the influence of posture.The attitude information that appearance accurately estimates VLC receivers is surveyed using fusion in VLC positioning fields first, mainly solving VLC positioning is easily influenceed by equipment posture and discontinuous problem is positioned in the case of optical signal is blocked.
Description
Technical field
The present invention relates to intelligent positioner and method, more particularly to merged based on MEMS sensor and VLC positioning
Single deck tape-recorder Kalman Filtering guider and method.
Background technology
With the development and application of indoor positioning technologies, the indoor positioning technologies based on visible light communication are also rapidly growing
And obtain extensive concern.In the environment of sufficient indoor light source, obtained by the detection of the equipment such as optical sensor by multiplex protocol
The optical signal of modulation, can be by different light signal data separatings, so that combining environmental parameter can be with by signal demodulation techniques
The distance or angle information of the positioning relatively each light source of target are calculated, can be completed finally by the positioning of location algorithm such as three sides
Target is positioned.
However, because the posture of intended recipient device can produce shake with target movement, will be brought to VLC positioning results
Large effect.On the other hand, optical signal is easily blocked in actual scene, it will cause positioning discontinuous.For previous
Problem, current scheme is mainly positioned jointly by multi sensor combination.For latter problem, the solution of main flow is
Target location is estimated by Kalman filtering or particle filter.
But these schemes be present：1) positioned compared to single-sensor, multi sensor combination location algorithm is complicated
And cost is higher；2) the wave filter fusion proposed at present in targeting scheme is premised on detector posture is steady, in actual field
Less stable in scape；3) in the scene that the situation that signal is blocked frequently occurs, only determining for wave filter is added by VLC data
The target location that position system speculates still is equipped with relatively large deviation with actual bit, and VLC positioning results are discontinuous, unsmooth.
The content of the invention
Goal of the invention：The influence that posture is positioned to VLC is eliminated there is provided a kind of to solve the deficiencies in the prior art, can be more
Benefit VLC positioning results are discontinuous, the rough shortcoming survey appearance positioning single deck tape-recorder that based on MEMS sensor and VLC positioning fusions
Graceful wave filter guider and method.
Technical scheme：Survey appearance based on MEMS sensor and VLC positioning fusions positions single Kalman filter guider,
Including MEMS sensor, inertial navigation system (Inertial Navigation System, INS) module, visible light communication
(Visible Light Communication, VLC) locating module, pedestrian's dead reckoning (Pedestrian Dead
Reckoning, PDR) locating module and survey appearance positioning single Kalman filter module；The MEMS sensor includes acceleration
Meter, gyroscope and magnetometer；
The input for surveying appearance positioning single Kalman filter module includes：The receiver that accelerometer is measured is in XYZ side
To angular velocity information of the receiver that measures of acceleration information and gyroscope on XYZ directions pass to INS mechanization moulds
Block, INS position, speed and attitude information that layout obtains receiver are carried out to it by INS modules；The receiver that magnetometer is measured
With respect to the angle information in east, south, west, north direction；The receiver that accelerometer is measured XYZ directions acceleration information to PDR
The PDR location informations and the VLC location informations of VLC locating modules output that locating module progress location estimation is obtained are flat by weighting
Positional information after；
The positional information for surveying appearance positioning single Kalman filter module output current time receiver, velocity information and
Attitude information gives INS mechanization modules, and the attitude information for exporting current time receiver is positioned to PDR locating modules or VLC
Module, output noise thermal compensation signal feeds back to accelerometer and gyroscope, while exporting the location information of current time receiver.
Wherein, whether the weight of the PDR location informations and VLC location informations optical signal in VLC locating modules is hidden
Keep off setting, if the weight of VLC positional informations is set to 0 by system when optical signal is blocked, only with PDR positional informations.
A kind of air navigation aid based on the guider, comprises the following steps：
(1) S-EKF state vector is set up；
(2) S-EKF system model is set up；
(3) S-EKF observational equation is set up；
(4) S-EKF filtering output location information.
Further, S-EKF state vector is in the step (1)：
X=[δ rn δvn ψ bg ba]T
Wherein, δ rn、δvn、ψ、bgAnd baIt is the site error vector, velocity error vector, attitude error of receiver respectively
Vector, gyroscope bias vector and accelerometer bias vector.
Further, the step (2) includes：
(21) the receiver data information that accelerometer and gyroscope are collected is input to INS modules and carries out mechanization
Algorithm process obtains receiver current location information, velocity information and attitude information and is input in S-EKF；Attitude matrix is entered
Row coordinate transform, its coordinate equation of transfer is：
Wherein,For rnFirst derivative,It is the position vector in navigational coordinate system,Represent latitude, λ
Represent that longitude and h represent height；It is vnFirst derivative, vnIt is three-dimensional velocity vector；gnGravity in navigational coordinate system to
Amount；Represent positional increment；Represent speed increment；fbIt is the specific force vector in carrier coordinate system；D-1Be one onWith
H 3 × 3 matrixes；It isFirst derivative,Be by INS mechanizations predict from carrier coordinate system to navigational coordinate system
Direction cosine matrix；WithRespectively angular velocity vector WithSkew symmetric matrix；AndWithRepresent the rotational angular velocity of angular speed and navigational coordinate system relative to ECEF coordinate system of earth autobiography；WithRepresent rotational angular velocity and navigational coordinate system angle of rotation relative to inertial coordinate of the carrier coordinate system relative to inertial coordinate
Speed；
(22) A-EKF system model isSpecific expansion formula is as follows：
Wherein, δ symbols represent the difference of error, i.e. actual value and system nominal value；WithRespectively
Represent δ rn、δvn、ψ、bgAnd baFirst derivative, fnIt is the specific force vector for projecting to navigational coordinate system；wgAnd waIt is sensor
Noise；τbgAnd τbaRepresent the correlation time of inertial navigation noise；wbgAnd wbaIt is driving noise, symbol "×" represents multiplication cross.
Further, the step (3) includes：
(31) the position detection equation of PDR or VLC outputs
When generation optical signal, which blocks VLC, can not export the situation of effective location information, PDR positional informations are determined as appearance is surveyed
The input quantity of position single Kalman filter module；It is according to the observational equation that PDR positional informations are set up：
WhereinWithThe latitude and longitude calculated for INS mechanizations；WithRespectively from PDR's
Latitude and longitude；And nλIt is measurement noise；
Otherwise, survey appearance positioning single Kalman filter module and receive VLC positional informations, and ignore PDR positional informations；According to
VLC output informations set up observational equation be：
Wherein,WithINS mechanizations and VLC position vector are come from respectively；δrnIt is site error vector；n1
It is to measure noise；
(32) magnetometer observational equation
Wave filter directly carries out posture renewal by magnetometer readings, and magnetometer observational equation is as follows：
Wherein, It is magnetometer readings vector, mnIt is the LMF vectors of calibration, n3It is noise.
Further, the step (4) includes：
(41) by system modelCarried out being transformed into following form according to the model form of Kalman filtering：
Wherein,
(42) observation model Z=HX+V correspondences below equation：
Wherein,H=[I3×3 03×3 03×3 03×3 03×3], V=[n1]；
When VLC exports inoperative position information, the positional information inputted using PDR, observational equation is：
When using VLC positional informations, observational equation is：
When using the filtering of magnetometer observational equation, magnetometer observational equation is filtered according to participation after Z=HX+V formal arguments
Ripple, wherein,H=[03×3 03×3[mn×]03×3 03×3],
(43) EKF recursion step is as follows：
(a) initial value is set
The initial value for making covariance matrix P is：
P0=diag ([var (r0) var(v0) var(ψ0) var(bg0) var(ba0)])
Wherein, r0、v0、ψ0、bg0And ba0Vector δ r are represented respectivelyn、δvn、ψ、bgAnd baInitial value, P0Represent each vector
The diagonal matrix that the covariance of initial vector is constituted；
If quantity of state X initial values are 0 vector；
(b) diagonal matrix at k moment is predicted using the diagonal matrix at k-1 moment, predictor formula is：
Pk'=FPk-1FT+Q
Wherein, Pk-1Represent the diagonal matrix at k-1 moment, PkThe predicted value of the diagonal matrix at ' expression k moment, Q=E
[(GW)(GW)T]；
(c) quantity of state at k moment is predicted using the quantity of state at k-1 moment, predictor formula is：
Wherein,The quantity of state at k-1 moment is represented,Represent the predicted value of k moment quantity of states；
(d) the kalman gain K ' at k moment is calculated, calculation formula is as follows：
Kk'=Pk′HT(HPk′HT+R)-1
Wherein, KkThe kalman gain at ' expression k moment, R=E [VVT]；
(e) optimal value of the quantity of state at k moment is estimated using the kalman gain at k moment, the optimal value is made as the k moment
Quantity of stateThen：
(f) optimal value of the diagonal matrix at k moment is estimated using the kalman gain at k moment, when making the optimal value as k
The diagonal matrix P at quarterk, then：
Pk=(I-KkH)Pk′；
(g) attitude information in output k moment quantity of states feeds back to VLC locating modules or PDR locating modules, during output k
Carve positional information, velocity information and attitude information in quantity of state and feed back to gyroscope in INS modules, output k moment quantity of states
Bias vector feeds back to accelerometer bias vector in gyroscope, output k moment quantity of states and feeds back to accelerometer；And export k
The location information of receiver in moment quantity of state；Meanwhile, k=k+1 is made, return to step (b) continues cycling through filtering and constantly updates shape
State amount, exports location information.
Beneficial effect：Compared with prior art, the single deck tape-recorder Germania of the invention based on MEMS sensor and VLC positioning fusions
Filtering guider and method have advantages below：1) survey appearance using fusion in VLC positioning fields first and accurately estimate that VLC is received
The attitude information of device, and eliminate the influence that posture is positioned to VLC；2) VLC positioning results can be made up discontinuous, rough scarce
Point；3) can be in the case where VLC signals be blocked using MEMS sensor information offer positioning result, and efficiency of algorithm is high, into
This is low；
Brief description of the drawings
Fig. 1 is to survey appearance positioning single Kalman filter guider structural representation；
Fig. 2 is the emulation positioning result CDF curves of 0 ° of the angle of pitch；
Fig. 3 is the emulation positioning result CDF curves of 5 ° of the angle of pitch；
Fig. 4 is the emulation positioning result CDF curves of 8 ° of the angle of pitch；
Fig. 5 be receiving device keep flat, tilt 5 °, tilt 8 ° when analysis of Positioning Error result.
Embodiment
Technical scheme is described in detail below in conjunction with the accompanying drawings.
As shown in figure 1, the survey appearance based on MEMS sensor and VLC positioning fusions positions single Kalman filter guider
Including：MEMS sensor, inertial navigation system (Inertial Navigation System, INS) module, visible light communication
(Visible Light Communication, VLC) locating module, pedestrian's dead reckoning (Pedestrian Dead
Reckoning, PDR) locating module and survey appearance positioning single Kalman filter module (S-EKF), wherein MEMS sensor includes
Accelerometer, gyroscope and magnetometer.
Acceleration signal of the accelerometer measures receiver in XYZ directions passes to PDR locating modules and carries out position all the way
Estimation, another road passes to INS modules, and angular velocity signal of the receiver that gyroscope is measured in XYZ directions passes to INS machineries
Orchestration module.INS modules carry out mechanization by mechanization algorithm to acceleration signal and angular velocity signal, obtain INS
Positional information, velocity information and attitude information.PDR locating modules carry out location estimation to acceleration signal, obtain PDR positions letter
Breath.Magnetometer measures receiver is with respect to the angle information in east, south, west, north direction, and VLC locating modules measure the position of receiver
Information.
Although in addition, VLC locating modules and PDR locating modules are positioned simultaneously, respective positioning result is not
Equivalence is input in survey appearance positioning single Kalman filter module.Generally, appearance positioning single Kalman filter module is surveyed
Mainly receive the positional information of VLC locating modules, and ignore the positional information of PDR locating modules.Only when generation optical signal hides
When the VLC locating modules such as gear can not export the situation of effective location information, the positional information of PDR modules can just be used as mainly defeated
Enter amount.
That is, the positional information of PDR locating modules and the positional information of VLC locating modules are passed through into the position after weighted average
Information is input to survey appearance positioning single Kalman filter module as unique positional information, when optical signal in VLC locating modules
The weight of the positional information of VLC locating modules is set to 0 by system when being blocked, only with the positional information of PDR modules.
Angle information and INS position information, velocity information that PDR positional informations, VLC positional informations, magnetometer are measured
Survey appearance positioning single Kalman filter module is input to together as input quantity with attitude information, device processing output after filtering is worked as
Positional information, velocity information and the attitude information of preceding reception device feed back to INS modules, export the appearance of current time receiver
State feedback of the information is to PDR modules or VLC locating modules, and output gyroscope bias vector feeds back to gyroscope, exports accelerometer
Bias vector feeds back to accelerometer, and exports the location information of current time receiver, while updating the state of subsequent time
Amount.
Survey appearance positioning and survey appearance positioning single Kalman filter (Single Extended Kalman Filter, S-EKF)
Focus on state vector, system equation, the modeling of state equation.S-EKF is mainly used to estimate the attitude information of receiver
(i.e.：Roll angle, the angle of pitch and azimuth) and positional information is (i.e.：Longitude, latitude and height).
A kind of air navigation aid based on above-mentioned guider, comprises the following steps：
(1) S-EKF state vector is set up
S-EKF state vector is defined as：
X=[δ rn δvn ψ bg ba]T (1)
Wherein, δ rn、δvn、ψ、bgAnd baIt is the site error vector, velocity error vector, attitude error of receiver respectively
Vector, gyroscope bias vector and accelerometer bias vector.
(2) S-EKF system model is set up
(21) the receiver data information that accelerometer and gyroscope are collected is input to INS modules and carries out mechanization
Algorithm process obtains receiver current location information, velocity information and attitude information and is input in S-EKF；Attitude matrix is entered
Row coordinate transform, its coordinate equation of transfer is：
Wherein,For rnFirst derivative,It is the position vector in navigational coordinate system,Represent latitude, λ
Represent that longitude and h represent height；It is vnFirst derivative, vnIt is three-dimensional velocity vector；gnGravity in navigational coordinate system to
Amount；Represent positional increment；Represent speed increment；fbIt is the specific force vector in carrier coordinate system；D-1Be one onWith
H 3 × 3 matrixes；It isFirst derivative,Be by INS mechanizations predict from carrier coordinate system to navigational coordinate system
Direction cosine matrix；WithRespectively angular velocity vector WithSkew symmetric matrix；AndWithRepresent the rotational angular velocity of angular speed and navigational coordinate system relative to ECEF coordinate system of earth autobiography；WithRepresent rotational angular velocity and navigational coordinate system angle of rotation relative to inertial coordinate of the carrier coordinate system relative to inertial coordinate
Speed.
(22) A-EKF system model isSpecific expansion formula is as follows：
Wherein, δ symbols represent the difference of error, i.e. actual value and system nominal value；WithRespectively
Represent δ rn、δvn、ψ、bgAnd baFirst derivative, fnIt is the specific force vector for projecting to navigational coordinate system；wgAnd waIt is sensor
Noise；τbgAnd τbaRepresent the correlation time of inertial navigation noise；wbgAnd wbaIt is driving noise, symbol "×" represents multiplication cross.
(3) S-EKF observational equation is set up
S-EKF observational equation is made up of two parts：1) the position detection equation 2 of PDR or VLC outputs) magnetometer observation
Equation.It should be noted that different sights should be used as input as input or VLC according to using PDR positional informations
Survey equation.
(31) the position detection equation of PDR or VLC outputs
When generation optical signal, which blocks VLC, can not export the situation of effective location information, PDR positional information, which is used as, surveys appearance
Position the main input quantity of single Kalman filter module；It is according to the observational equation that PDR positions are set up：
WhereinWithThe latitude and longitude calculated for INS mechanizations；WithLatitude and warp from PDR
Degree；And nλIt is measurement noise.
Otherwise, appearance positioning single Kalman filter module primary recipient VLC positional information is surveyed, and ignores PDR positioning
Information；It is according to the observational equation that VLC output informations are set up：
Wherein,WithINS mechanizations and VLC position vector are come from respectively；δrnIt is site error vector；n1
It is to measure noise.
Whether the weight of the PDR location informations and VLC location informations optical signal in VLC locating modules is blocked to set
It is fixed, if the weight of VLC positional informations is set to 0 by system when optical signal is blocked, only with PDR positional informations.
(32) magnetometer observational equation
Wave filter directly carries out posture renewal by magnetometer readings, and magnetometer observational equation is as follows：
Wherein, It is magnetometer readings vector, mnIt is the LMF vectors of calibration, n3It is noise.
(4) S-EKF is filtered
In order to further realize EKF, it is necessary to by system model (formula (3)) and observation model (formula
(4)~(6)) it is updated to the time update equation and measurement updaue equation of EKF.First first by above-mentioned middle system
Model and observation model enter line translation according to the model form of Kalman filtering, so as to be corresponded with the variable in model.
(41) by system model：The form can be by formula (3) conversion come the equation after converting such as
Under：
Wherein,
(42) observation model：Z=HX+V, correspondence below equation：
Wherein,H=[I3×3 03×3 03×3 03×3 03×3], V=[n1]；
When VLC exports inoperative position information, the positional information inputted using PDR, observational equation is：
When the positional information using VLC, observational equation is：
When using the filtering of magnetometer observational equation, need also exist for becoming magnetometer observational equation (6) according to Z=HX+V forms
Filtering is participated in after changing.Wherein,H=[03×3 03×3[mn×]03×3 03×3],
(43) EKF
After determining each variable, it is possible to according to the time update equation of EKF and measurement updaue side
Journey recursion is filtered.EKF recursion step is as follows：
(a) initial value is set
The initial value for making covariance matrix P is：
P0=diag ([var (r0) var(v0) var(ψ0) var(bg0) var(ba0)]) (11)
Wherein, r0、v0、ψ0、bg0And ba0Vector δ r are represented respectivelyn、δvn、ψ、bgAnd baInitial value, P0Represent each vector
The diagonal matrix that the covariance of initial vector is constituted.
If quantity of state X initial values are 0 vector.
(b) diagonal matrix at k moment is predicted using the diagonal matrix at k-1 moment, predictor formula is：
Pk'=FPk-1FT+Q (12)
Wherein, Pk-1Represent the diagonal matrix at k-1 moment, PkThe predicted value of the diagonal matrix at ' expression k moment, Q=E
[(GW)(GW)T]。
(c) quantity of state at k moment is predicted using the quantity of state at k-1 moment, predictor formula is：
Wherein,The quantity of state at k-1 moment is represented,Represent the predicted value of k moment quantity of states.
(d) the kalman gain K ' at k moment is calculated, calculation formula is as follows：
Kk'=Pk′HT(HPk′HT+R)-1 (14)
Wherein, KkThe kalman gain at ' expression k moment, R=E [VVT]。
(e) optimal value of the quantity of state at k moment is estimated using the kalman gain at k moment, the optimal value is made as the k moment
Quantity of stateThen：
(f) optimal value of the diagonal matrix at k moment is estimated using the kalman gain at k moment, when making the optimal value as k
The diagonal matrix P at quarterk, then：
Pk=(I-KkH)Pk′ (16)
(g) attitude information in output k moment quantity of states feeds back to VLC locating modules and PDR locating modules, during output k
Carve positional information, velocity information and attitude information in quantity of state and feed back to gyroscope in INS modules, output k moment quantity of states
Bias vector feeds back to accelerometer bias vector in gyroscope, output k moment quantity of states and feeds back to accelerometer；And export k
The location information of receiver in moment quantity of state；
(h) k=k+1 is made, return to step (b) continues cycling through filtering and constantly updates quantity of state, exports location information.
Wherein, the equation in step (b), (c) is time update equation, and equation is measurement updaue side in (d), (e), (f)
Journey.
Filtering except it is initial when carried out for (a)~(g), can be expressed as step (b)~(h) from circulating second
Circulation is carried out, and is continuously updated over time quantity of state, and the attitude information in state can feed back to VLC locating modules and PDR positioning
Module, to correct the influence of posture, location information (positional information and velocity information) can be exported outwards.Because state vector is position
Put error vector and velocity error vector, it is therefore desirable to add the INS position velocity information at that moment of output position information
The error vector of wave filter output, is accordingly changed into position and speed is exported.
The fused filtering device (surveying appearance positioning single Kalman filter) is accurate using fusion survey appearance in VLC positioning fields first
Estimate the attitude information of receiver, and eliminate the influence that posture is positioned to VLC.Error equation conduct based on inertial navigation mechanization
The system equation of fused filtering device.And observational equation then includes accelerometer observed quantity and updates (the position sight of PDR or VLC outputs
Survey equation) and magnetometer observed quantity renewal.Fused filtering device exports the posture of receiver to VLC locating modules to correct posture
Influence.In the design of positioning filter, the positional information of two dimensional surface is as system mode vector, based on pedestrian's dead reckoning
Error equation as system equation, and VLC positioning result is then as the observational equation of positioning filter.
Technical principle：
In VLC locating modules, location algorithm typically uses three side location algorithms.Its principle is to utilize many of indoor top
Small cup lamp substitutes into optical signal and passed as emission source, typically at least three LEDs, the light signal strength measured in real time by receiver
Broadcast model and estimate target point to the distance of every lamp, position location is estimated finally by simultaneous equations.However, working as optical signal
When being blocked, detector can not obtain whole signals, and the position solved can seriously exceed error range, and such case can pass through
Threshold value is excluded, so system not output position information in this case, causes positioning result discontinuous, unsmooth.In addition,
Often there is the random noises such as shot noise and thermal noise in view of VLC systems, the estimation of distance often has certain error, because
The movement locus that this position estimated is formed can have the features such as precipitous change, and the motion that this does not often meet realistic objective is special
Levy.Because in target following, priori should represent that positioning track is smooth, the state at target current time and last moment
State it is related, and filtering method can take into account priori value, so that positioning track is more smooth.
VLC positioning general at present often assumes that equipment is to keep flat posture, so as to simplify location algorithm.But in reality
Equipment can produce shake, inclination in situation.The random inclination of equipment can change the incidence angle of optical signal, so as to cause the light measured
Signal intensity changes.If can not accurately estimate the posture of equipment, the light signal strength change of this part will turn into system
A part for error, causes position error to become big.In order to illustrate influence degree of the attitude error for VLC positioning precisions, we
The precision that VLC is positioned in the case of the different angles of pitch by contrast simulation.The emulation experiment is determined receiver run-off the straight
The situation that position algorithm is not corrected is simulated, and all receivers are simulated with two kinds of different inclination conditions, positioning result CDF
Respectively as shown in Figure 3 and Figure 4, Fig. 2 represents the positioning result of the non-run-off the straight of equipment to curve.Experimental result display receiver pitching
Maximum positioning error respectively may be about 0.13m and 0.21m when angle is 5 ° and 8 °.Positioning knot when contrast receiver is tilted and kept flat
Really, as shown in Figure 5, it can be seen that when receiver is tilted, if be not modified to receiver posture, position error meeting
Significant increase.The simulation results show accurately estimates that the posture of receiver has weight to the performance for improving visible ray alignment system
Want meaning.Therefore, MEMS Attitude estimation modules are introduced, accurate attitude information is passed into VLC locating modules carries out posture school
Standard, can significantly reduce influence of the posture to VLC positioning results.
Due to optical signal, in the case where being blocked, VLC positioning can not be carried out, therefore introducing PDR locating modules are determined
Position.PDR positioning can be fully utilized MEMS sensor signal and come material calculation and direction, be pushed away using the position of pedestrian's last moment
Measure the current position of pedestrian.By iterating, you can obtain the movement locus of pedestrian.Therefore, when signal occurs for VLC positioning
When blocking, it is impossible to outgoing position result, emerging system can be made up using the positioning result of PDR locating modules.
Claims (7)
1. the single deck tape-recorder Kalman Filtering guider based on MEMS sensor and VLC positioning fusions, it is characterised in that：Passed including MEMS
Sensor, INS modules, VLC locating modules, PDR locating modules and survey appearance positioning single Kalman filter module；The MEMS sensings
Device includes accelerometer, gyroscope and magnetometer；
The input for surveying appearance positioning single Kalman filter module includes：The receiver that accelerometer is measured is in XYZ directions
Angular velocity information of the receiver that acceleration information and gyroscope are measured on XYZ directions passes to INS modules, by INS modules
INS position, speed and attitude information that layout obtains receiver are carried out to it；The receiver that magnetometer is measured relatively east, south,
West, the north to angle information；Acceleration information of the receiver that accelerometer is measured in XYZ directions enters to PDR locating modules
The VLC location informations for PDR location informations and VLC the locating modules output that row location estimation is obtained pass through the position after weighted average
Information；
It is described to survey positional information, velocity information and posture that appearance positioning single Kalman filter module exports current time receiver
Information gives INS modules, exports the attitude information of current time receiver to PDR locating modules or VLC locating modules, output noise
Thermal compensation signal feeds back to accelerometer and gyroscope, while exporting the location information of current time receiver.
2. guider according to claim 1, it is characterised in that：The power of the PDR location informations and VLC location informations
Whether weight optical signal in VLC locating modules is blocked to set, if system is by the power of VLC positional informations when optical signal is blocked
0 is reset to, only with PDR positional informations.
3. a kind of air navigation aid based on guider described in claim 1 or 2, it is characterised in that comprise the following steps：
(1) S-EKF state vector is set up；
(2) S-EKF system model is set up；
(3) S-EKF observational equation is set up；
(4) S-EKF filtering output location information.
4. a kind of air navigation aid according to claim 3, it is characterised in that：S-EKF state vector in the step (1)
For：
X=[δ rn δvn ψ bg ba]T
Wherein, δ rn、δvn、ψ、bgAnd baBe respectively the site error vector of receiver, velocity error vector, attitude error vector,
Gyroscope bias vector and accelerometer bias vector.
5. a kind of air navigation aid according to claim 4, it is characterised in that the step (2) includes：
(21) the receiver data information that accelerometer and gyroscope are collected is input to INS modules and carries out mechanization algorithm
Processing obtains receiver current location information, velocity information and attitude information and is input in S-EKF；Attitude matrix is sat
Mark is converted, and its coordinate equation of transfer is：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msup>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>C</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>b</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<mi>D</mi>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<msup>
<mi>v</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msup>
<mi>f</mi>
<mi>b</mi>
</msup>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<msubsup>
<mi>&Omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&Omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<msup>
<mi>v</mi>
<mi>n</mi>
</msup>
<mo>+</mo>
<msup>
<mi>g</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&Omega;</mi>
<mrow>
<mi>i</mi>
<mi>b</mi>
</mrow>
<mi>b</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>&Omega;</mi>
<mrow>
<mi>i</mi>
<mi>n</mi>
</mrow>
<mi>b</mi>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
Wherein,For rnFirst derivative,It is the position vector in navigational coordinate system,Latitude is represented, λ is represented
Longitude and h represent height；It is vnFirst derivative, vnIt is three-dimensional velocity vector；gnIt is the gravity vector in navigational coordinate system；Represent positional increment；Represent speed increment；fbIt is the specific force vector in carrier coordinate system；D-1Be one onWith h's
3 × 3 matrixes；It isFirst derivative,It is the side from carrier coordinate system to navigational coordinate system predicted by INS mechanizations
To cosine matrix；WithRespectively angular velocity vector WithSkew symmetric matrix；AndWithRepresent the rotational angular velocity of angular speed and navigational coordinate system relative to ECEF coordinate system of earth autobiography；WithTable
Show rotational angular velocity and navigational coordinate system rotational angular velocity relative to inertial coordinate of the carrier coordinate system relative to inertial coordinate；
(22) A-EKF system model isSpecific expansion formula is as follows：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msup>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>b</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>g</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>b</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>a</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&times;</mo>
<msup>
<mi>&delta;r</mi>
<mi>n</mi>
</msup>
<mo>+</mo>
<msup>
<mi>&delta;v</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msup>
<mi>&delta;v</mi>
<mi>n</mi>
</msup>
<mo>+</mo>
<msup>
<mi>f</mi>
<mi>n</mi>
</msup>
<mo>&times;</mo>
<mi>&psi;</mi>
<mo>+</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>b</mi>
<mi>a</mi>
</msub>
<mo>+</mo>
<msub>
<mi>w</mi>
<mi>a</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<mi>&psi;</mi>
<mo>-</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>b</mi>
<mi>g</mi>
</msub>
<mo>+</mo>
<msub>
<mi>w</mi>
<mi>g</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>/</mo>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>b</mi>
<mi>g</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>b</mi>
<mi>g</mi>
</msub>
<mo>+</mo>
<msub>
<mi>w</mi>
<mrow>
<mi>b</mi>
<mi>g</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>/</mo>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>b</mi>
<mi>a</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>b</mi>
<mi>a</mi>
</msub>
<mo>+</mo>
<msub>
<mi>w</mi>
<mrow>
<mi>b</mi>
<mi>a</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
Wherein, δ symbols represent the difference of error, i.e. actual value and system nominal value；WithRepresent respectively
δrn、δvn、ψ、bgAnd baFirst derivative, fnIt is the specific force vector for projecting to navigational coordinate system；wgAnd waIt is sensor noise；
τbgAnd τbaRepresent the correlation time of inertial navigation noise；wbgAnd wbaIt is driving noise, symbol "×" represents multiplication cross.
6. a kind of air navigation aid according to claim 5, it is characterised in that the step (3) includes：
(31) the position detection equation of PDR or VLC outputs
When generation optical signal, which blocks VLC, can not export the situation of effective location information, PDR positional informations, which are used as, surveys appearance positioning list
The input quantity of Kalman filter module；It is according to the observational equation that PDR positional informations are set up：
WhereinWithThe latitude and longitude calculated for INS mechanizations；WithLatitude and warp respectively from PDR
Degree；And nλIt is measurement noise；
Otherwise, survey appearance positioning single Kalman filter module and receive VLC positional informations, and ignore PDR positional informations；According to VLC
Output information set up observational equation be：
<mrow>
<msup>
<mover>
<mi>r</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
</msup>
<mo>-</mo>
<msup>
<mover>
<mi>r</mi>
<mo>~</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<msup>
<mi>&delta;r</mi>
<mi>n</mi>
</msup>
<mo>+</mo>
<msub>
<mi>n</mi>
<mn>1</mn>
</msub>
</mrow>
Wherein,WithINS mechanizations and VLC position vector are come from respectively；δrnIt is site error vector；n1The amount of being
Survey noise；
(32) magnetometer observational equation
Wave filter directly carries out posture renewal by magnetometer readings, and magnetometer observational equation is as follows：
<mrow>
<msup>
<mi>&delta;m</mi>
<mi>n</mi>
</msup>
<mo>=</mo>
<mo>&lsqb;</mo>
<msup>
<mi>m</mi>
<mi>n</mi>
</msup>
<mo>&times;</mo>
<mo>&rsqb;</mo>
<mi>&psi;</mi>
<mo>+</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msub>
<mi>n</mi>
<mn>3</mn>
</msub>
</mrow>
Wherein, It is magnetometer readings vector, mnIt is the LMF vectors of calibration, n3It is noise.
7. a kind of air navigation aid according to claim 6, it is characterised in that the step (4) includes：
(41) by system modelCarried out being transformed into following form according to the model form of Kalman filtering：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msup>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>b</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>g</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>b</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>a</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>&lsqb;</mo>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&times;</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>&lsqb;</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>&lsqb;</mo>
<msup>
<mi>f</mi>
<mi>n</mi>
</msup>
<mo>&times;</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>&lsqb;</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>/</mo>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>b</mi>
<mi>g</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>/</mo>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>b</mi>
<mi>a</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<mi>&delta;r</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<mi>&delta;v</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&psi;</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>b</mi>
<mi>g</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>b</mi>
<mi>a</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msub>
<mi>w</mi>
<mi>a</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msub>
<mi>w</mi>
<mi>g</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>b</mi>
<mi>g</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>b</mi>
<mi>a</mi>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
Wherein,
<mrow>
<mover>
<mi>X</mi>
<mo>&CenterDot;</mo>
</mover>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msup>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>b</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>g</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>b</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>a</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mi>F</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>&lsqb;</mo>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&times;</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>&lsqb;</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>&lsqb;</mo>
<msup>
<mi>f</mi>
<mi>n</mi>
</msup>
<mo>&times;</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>&lsqb;</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>e</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>e</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
<mtd>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>/</mo>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>b</mi>
<mi>g</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>/</mo>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>b</mi>
<mi>a</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
<mrow>
<mi>G</mi>
<mi>W</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msub>
<mi>w</mi>
<mi>a</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msub>
<mi>w</mi>
<mi>g</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>b</mi>
<mi>g</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>b</mi>
<mi>a</mi>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
(42) observation model Z=HX+V correspondences below equation：
<mrow>
<mo>&lsqb;</mo>
<msup>
<mover>
<mi>r</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
</msup>
<mo>-</mo>
<msup>
<mover>
<mi>r</mi>
<mo>~</mo>
</mover>
<mi>n</mi>
</msup>
<mo>&rsqb;</mo>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<mi>&delta;r</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<mi>&delta;v</mi>
<mi>n</mi>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&psi;</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>b</mi>
<mi>g</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>b</mi>
<mi>a</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mo>&lsqb;</mo>
<msub>
<mi>n</mi>
<mn>1</mn>
</msub>
<mo>&rsqb;</mo>
</mrow>
Wherein,H=[I3×3 03×3 03×3 03×3 03×3], V=[n1]；
When VLC exports inoperative position information, the positional information inputted using PDR, observational equation is：
When using VLC positional informations, observational equation is：
<mrow>
<msubsup>
<mover>
<mi>r</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mover>
<mi>r</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>V</mi>
<mi>L</mi>
<mi>C</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msup>
<mi>&delta;r</mi>
<mi>n</mi>
</msup>
<mo>+</mo>
<msub>
<mi>n</mi>
<mn>1</mn>
</msub>
</mrow>
When using the filtering of magnetometer observational equation, magnetometer observational equation is filtered according to participation after Z=HX+V formal arguments, its
In,H=[03×3 03×3 [mn×] 03×3 03×3],
(43) EKF recursion step is as follows：
(a) initial value is set
The initial value for making covariance matrix P is：
P0=diag ([var (r0) var(v0) var(ψ0) var(bg0) var(ba0)])
Wherein, r0、v0、ψ0、bg0And ba0Vector δ r are represented respectivelyn、δvn、ψ、bgAnd baInitial value, P0Represent that each vector is initial
The diagonal matrix that the covariance of vector is constituted；
If quantity of state X initial values are 0 vector；
(b) diagonal matrix at k moment is predicted using the diagonal matrix at k-1 moment, predictor formula is：
P′k=FPk-1FT+Q
Wherein, Pk-1Represent the diagonal matrix at k-1 moment, PkThe predicted value of the diagonal matrix at ' expression k moment, Q=E [(GW) (GW
)T]；
(c) quantity of state at k moment is predicted using the quantity of state at k-1 moment, predictor formula is：
<mrow>
<msubsup>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
<mo>&prime;</mo>
</msubsup>
<mo>=</mo>
<mi>F</mi>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
Wherein,The quantity of state at k-1 moment is represented,Represent the predicted value of k moment quantity of states；
(d) the kalman gain K ' at k moment is calculated, calculation formula is as follows：
K′k=P 'kHT(HP′kHT+R)-1
Wherein, K 'kRepresent the kalman gain at k moment, R=E [VVT]；
(e) optimal value of the quantity of state at k moment is estimated using the kalman gain at k moment, the optimal value is made as the shape at k moment
State amountThen：
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
</msub>
<mo>=</mo>
<msubsup>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
<mo>-</mo>
</msubsup>
<mo>+</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>Z</mi>
<mi>k</mi>
</msub>
<mo>-</mo>
<mi>H</mi>
<msubsup>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
<mo>-</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
(f) optimal value of the diagonal matrix at k moment is estimated using the kalman gain at k moment, the optimal value is made as the k moment
Diagonal matrix Pk, then：
Pk=(I-KkH)P′k；
(g) attitude information in output k moment quantity of states feeds back to VLC locating modules or PDR locating modules, exports k moment shapes
Positional information, velocity information and attitude information in state amount feed back to gyroscope deviation in INS modules, output k moment quantity of states
Vector feedback exports accelerometer bias vector in k moment quantity of states and feeds back to accelerometer to gyroscope；And export the k moment
The location information of receiver in quantity of state；Meanwhile, k=k+1 is made, return to step (b) continues cycling through filtering and constantly updates quantity of state,
Export location information.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710505803.6A CN107289932B (en) | 2017-06-28 | 2017-06-28 | Single deck tape-recorder Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710505803.6A CN107289932B (en) | 2017-06-28 | 2017-06-28 | Single deck tape-recorder Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107289932A true CN107289932A (en) | 2017-10-24 |
| CN107289932B CN107289932B (en) | 2019-08-20 |
Family
ID=60099369
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710505803.6A Active CN107289932B (en) | 2017-06-28 | 2017-06-28 | Single deck tape-recorder Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107289932B (en) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107888289A (en) * | 2017-11-14 | 2018-04-06 | 东南大学 | The indoor orientation method and platform merged based on visible light communication with inertial sensor |
| CN108803673A (en) * | 2018-05-07 | 2018-11-13 | 约肯机器人（上海）有限公司 | Directional controlling method and device |
| CN109814133A (en) * | 2019-03-07 | 2019-05-28 | 上海华测导航技术股份有限公司 | GNSS receiver inclinometric system, method, apparatus and storage medium |
| CN109883416A (en) * | 2019-01-23 | 2019-06-14 | 中国科学院遥感与数字地球研究所 | A kind of localization method and device of the positioning of combination visible light communication and inertial navigation positioning |
| CN110031803A (en) * | 2019-04-04 | 2019-07-19 | 中国科学院数学与***科学研究院 | The fusion and positioning method of double infrared sensors with Stochastic Measurement Noises |
| CN110132257A (en) * | 2019-05-15 | 2019-08-16 | 吉林大学 | Human body behavior prediction method based on Fusion |
| CN111811500A (en) * | 2020-05-06 | 2020-10-23 | 北京嘀嘀无限科技发展有限公司 | Target object pose estimation method and device, storage medium and electronic equipment |
| CN114370870A (en) * | 2022-01-05 | 2022-04-19 | 中国兵器工业计算机应用技术研究所 | Filter updating information screening method suitable for pose measurement Kalman filtering |
| CN114895241A (en) * | 2022-05-09 | 2022-08-12 | 知微空间智能科技(苏州)有限公司 | Elastic fusion positioning method and device based on data and model combined drive |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103175529A (en) * | 2013-03-01 | 2013-06-26 | 上海美迪索科电子科技有限公司 | Pedestrian inertial positioning system based on indoor magnetic field feature assistance |
| CN104819716A (en) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (micro-electromechanical system) |
| CN105222772A (en) * | 2015-09-17 | 2016-01-06 | 泉州装备制造研究所 | A kind of high-precision motion track detection system based on Multi-source Information Fusion |
| WO2016040166A1 (en) * | 2014-09-08 | 2016-03-17 | Invensense, Inc. | Method and apparatus for using map information aided enhanced portable navigation |
| CN105652306A (en) * | 2016-01-08 | 2016-06-08 | 重庆邮电大学 | Dead reckoning-based low-cost Big Dipper and MEMS tight-coupling positioning system and method |
| CN106225784A (en) * | 2016-06-13 | 2016-12-14 | 国家***第二海洋研究所 | Based on low cost Multi-sensor Fusion pedestrian's dead reckoning method |
-
2017
- 2017-06-28 CN CN201710505803.6A patent/CN107289932B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103175529A (en) * | 2013-03-01 | 2013-06-26 | 上海美迪索科电子科技有限公司 | Pedestrian inertial positioning system based on indoor magnetic field feature assistance |
| WO2016040166A1 (en) * | 2014-09-08 | 2016-03-17 | Invensense, Inc. | Method and apparatus for using map information aided enhanced portable navigation |
| CN104819716A (en) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (micro-electromechanical system) |
| CN105222772A (en) * | 2015-09-17 | 2016-01-06 | 泉州装备制造研究所 | A kind of high-precision motion track detection system based on Multi-source Information Fusion |
| CN105652306A (en) * | 2016-01-08 | 2016-06-08 | 重庆邮电大学 | Dead reckoning-based low-cost Big Dipper and MEMS tight-coupling positioning system and method |
| CN106225784A (en) * | 2016-06-13 | 2016-12-14 | 国家***第二海洋研究所 | Based on low cost Multi-sensor Fusion pedestrian's dead reckoning method |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107888289A (en) * | 2017-11-14 | 2018-04-06 | 东南大学 | The indoor orientation method and platform merged based on visible light communication with inertial sensor |
| CN107888289B (en) * | 2017-11-14 | 2020-08-11 | 东南大学 | Indoor positioning method and platform based on fusion of visible light communication and inertial sensor |
| CN108803673A (en) * | 2018-05-07 | 2018-11-13 | 约肯机器人（上海）有限公司 | Directional controlling method and device |
| CN109883416A (en) * | 2019-01-23 | 2019-06-14 | 中国科学院遥感与数字地球研究所 | A kind of localization method and device of the positioning of combination visible light communication and inertial navigation positioning |
| CN109814133A (en) * | 2019-03-07 | 2019-05-28 | 上海华测导航技术股份有限公司 | GNSS receiver inclinometric system, method, apparatus and storage medium |
| CN110031803A (en) * | 2019-04-04 | 2019-07-19 | 中国科学院数学与***科学研究院 | The fusion and positioning method of double infrared sensors with Stochastic Measurement Noises |
| CN110031803B (en) * | 2019-04-04 | 2020-11-27 | 中国科学院数学与***科学研究院 | Fusion positioning method of double infrared sensors with random measurement noise |
| CN110132257A (en) * | 2019-05-15 | 2019-08-16 | 吉林大学 | Human body behavior prediction method based on Fusion |
| CN111811500A (en) * | 2020-05-06 | 2020-10-23 | 北京嘀嘀无限科技发展有限公司 | Target object pose estimation method and device, storage medium and electronic equipment |
| CN114370870A (en) * | 2022-01-05 | 2022-04-19 | 中国兵器工业计算机应用技术研究所 | Filter updating information screening method suitable for pose measurement Kalman filtering |
| CN114370870B (en) * | 2022-01-05 | 2024-04-12 | 中国兵器工业计算机应用技术研究所 | Filter update information screening method suitable for pose measurement Kalman filtering |
| CN114895241A (en) * | 2022-05-09 | 2022-08-12 | 知微空间智能科技(苏州)有限公司 | Elastic fusion positioning method and device based on data and model combined drive |
| CN114895241B (en) * | 2022-05-09 | 2024-05-03 | 知微空间智能科技(苏州)有限公司 | Elastic fusion positioning method and device based on data and model combined driving |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107289932B (en) | 2019-08-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107289932B (en) | Single deck tape-recorder Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion | |
| CN107289933B (en) | Double card Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion | |
| CN107270898B (en) | Double particle filter navigation devices and method based on MEMS sensor and VLC positioning fusion | |
| WO2020062791A1 (en) | Sins/dvl-based underwater anti-shaking alignment method for deep-sea underwater vehicle | |
| CN105628026B (en) | A kind of positioning and orientation method and system of mobile object | |
| Heo et al. | EKF-based visual inertial navigation using sliding window nonlinear optimization | |
| CN104729506B (en) | A kind of unmanned plane Camera calibration method of visual information auxiliary | |
| CN103499350B (en) | Vehicle high-precision localization method and the device of multi-source information is merged under GPS blind area | |
| CN108731670A (en) | Inertia/visual odometry combined navigation locating method based on measurement model optimization | |
| CN110243358A (en) | The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion | |
| CN107246872A (en) | Single-particle filtering guider and method based on MEMS sensor and VLC positioning fusions | |
| CN111426320B (en) | Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter | |
| CN106969784B (en) | A kind of combined error emerging system for concurrently building figure positioning and inertial navigation | |
| CN107192387A (en) | A kind of combined positioning method based on Unscented kalman filtering | |
| CN101187567A (en) | Optical fiber gyroscope strap-down inertial navigation system initial posture determination method | |
| CN108151737A (en) | A kind of unmanned plane bee colony collaborative navigation method under the conditions of the mutual observed relationships of dynamic | |
| CN106197406A (en) | A kind of based on inertial navigation with the fusion method of RSSI wireless location | |
| CN109813306A (en) | A kind of unmanned vehicle planned trajectory satellite location data confidence level calculation method | |
| CN117289322B (en) | High-precision positioning algorithm based on IMU, GPS and UWB | |
| CN107270896A (en) | A kind of pedestrian's positioning and trace tracking method and system | |
| CN109959374A (en) | A kind of full-time reverse smooth filtering method of whole process of pedestrian's inertial navigation | |
| CN108871325B (en) | A kind of WiFi/MEMS combination indoor orientation method based on two layers of Extended Kalman filter | |
| CN112747748A (en) | Pilot AUV navigation data post-processing method based on reverse solution | |
| Hu et al. | Fusion of vision, GPS and 3D gyro data in solving camera registration problem for direct visual navigation | |
| CN106054227B (en) | Pseudorange difference list star high dynamic localization method under inertial navigation auxiliary |
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 |