CN107289932B - Single deck tape-recorder Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion - Google Patents

Single deck tape-recorder Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion Download PDF

Info

Publication number
CN107289932B
CN107289932B CN201710505803.6A CN201710505803A CN107289932B CN 107289932 B CN107289932 B CN 107289932B CN 201710505803 A CN201710505803 A CN 201710505803A CN 107289932 B CN107289932 B CN 107289932B
Authority
CN
China
Prior art keywords
vlc
vector
moment
information
pdr
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.)
Active
Application number
CN201710505803.6A
Other languages
Chinese (zh)
Other versions
CN107289932A (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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN201710505803.6A priority Critical patent/CN107289932B/en
Publication of CN107289932A publication Critical patent/CN107289932A/en
Application granted granted Critical
Publication of CN107289932B publication Critical patent/CN107289932B/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
    • 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

Abstract

The invention discloses a kind of single deck tape-recorder Kalman Filtering navigation device merged based on MEMS sensor and VLC positioning and methods, including MEMS sensor, INS module, VLC locating module, PDR locating module and survey appearance to position single Kalman filter module；System equation the present invention is based on the error equation of INS inertial navigation mechanization as fused filtering device, observational equation include that VLC positioning information update, PDR positioning information update and magnetometer observed quantity update.The posture that fused filtering device exports VLC receiver gives VLC locating module, and the posture for exporting PDR equipment corrects the influence of posture to PDR locating module.The posture information that appearance accurately estimates VLC receiver is surveyed using fusion in VLC positioning field for the first time, mainly solves the problems, such as that VLC positioning is easy to be influenced and positioned when optical signal is blocked discontinuous by equipment posture.

Description

Based on MEMS sensor and VLC positioning fusion single deck tape-recorder Kalman Filtering navigation device and Method
Technical field
The present invention relates to intelligent positioner and methods, more particularly to what is merged based on MEMS sensor and VLC positioning Single deck tape-recorder Kalman Filtering navigation device and method.
Background technique
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, detect to obtain by multiplex protocol by equipment such as optical sensors 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 for calculating the positioning relatively each light source of target can be completed finally by the positioning of location algorithm such as three sides Target positioning.
However, the posture due to intended recipient device can generate shake as target is mobile, VLC positioning result will be brought Large effect.On the other hand, optical signal is easily blocked in actual scene, it will causes positioning discontinuous.For previous Problem, current scheme are mainly co-located by multi sensor combination.For latter problem, the solution of mainstream is Target position is estimated by Kalman filtering or particle filter.
But there are some problems for these schemes: 1) positioning compared to single-sensor, multi sensor combination location algorithm is complicated And higher cost；2) the filter fusion proposed in locating scheme at present is premised on detector posture is steady, in actual field Stability is poor in scape；3) in the scene frequently occurred the case where signal is blocked, only add determining for filter by VLC data The target position that position system speculates still is equipped with relatively large deviation with actual bit, and VLC positioning result is discontinuous, unsmooth.
Summary of the invention
Goal of the invention: to solve the deficiencies in the prior art, the influence that posture positions VLC can be eliminated by providing one kind, can be more Mend the survey appearance positioning single deck tape-recorder that based on MEMS sensor and VLC positioning fusion of discontinuous, the rough disadvantage of VLC positioning result Graceful filter navigation device and method.
Technical solution: the survey appearance based on MEMS sensor and VLC positioning fusion positions single Kalman filter navigation device, 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 position 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 measures of accelerometer in the side XYZ To angular velocity information of the receiver that measures of acceleration information and gyroscope on the direction XYZ pass to INS mechanization mould Block is carried out layout to it by INS module and obtains INS position, speed and the posture information of receiver；The receiver that magnetometer measures The angle information in opposite east, south, west, north direction；The receiver that accelerometer measures the direction XYZ acceleration information to PDR Locating module carries out the PDR location information that location estimation obtains and the VLC location information of VLC locating module output is flat by weighting Location information after；
It is described survey the appearance positioning single Kalman filter module output location information of current time receiver, velocity information and Posture information gives INS mechanization module, and the posture information for exporting current time receiver is positioned to PDR locating module or VLC Module, output noise thermal compensation signal feed back to accelerometer and gyroscope, while exporting the location information of current time receiver.
Wherein, whether the weight of the PDR location information and VLC location information is hidden by optical signal in VLC locating module Gear is to set, if the weight of VLC location information is set as 0 by system when optical signal is blocked, only with PDR location information.
A kind of air navigation aid based on the navigation device, comprising the following steps:
(1) state vector of S-EKF is established；
(2) system model of S-EKF is established；
(3) observational equation of S-EKF is established；
(4) S-EKF filtering output location information.
Further, in the step (1) S-EKF state vector are as follows:
X=[δ rn δvn ψ bg ba]T
Wherein, δ rn、δvn、ψ、bgAnd baIt is location error vector, the velocity error vector, attitude error of receiver respectively Vector, gyroscope bias vector and accelerometer bias vector.
Further, the step (2) includes:
(21) accelerometer and the collected receiver data information input of gyroscope are subjected to mechanization to INS module Algorithm process obtains receiver current location information, velocity information and posture information and is input in S-EKF；To attitude matrix into Row coordinate transform, coordinate equation of transfer are as follows:
Wherein,For rnFirst derivative,It is the position vector in navigational coordinate system,Indicate latitude, λ indicates that longitude and h indicate height；It is vnFirst derivative, vnIt is three-dimensional velocity vector；gnIt is the gravity in navigational coordinate system Vector；Indicate positional increment；Indicate speed increment；fbIt is the specific force vector in carrier coordinate system；D-1Be one aboutWith 3 × 3 matrixes of h；It isFirst derivative,It is to be predicted by INS mechanization slave carrier coordinate system to navigation seat Mark the direction cosine matrix of system；WithRespectively angular velocity vector WithSkew symmetry square Battle array；AndWithIndicate the angle of rotation speed of the angular speed and navigational coordinate system of earth autobiography relative to ECEF coordinate system Degree；WithIndicate carrier coordinate system relative to inertial coordinate rotational angular velocity and navigational coordinate system relative to inertial coordinate Rotational angular velocity；
(22) system model of A-EKF isSpecific expansion formula is as follows:
Wherein, δ symbol indicates error, the i.e. difference of true value and system nominal value；WithPoint It Biao Shi not δ rn、δvn、ψ、bgAnd baFirst derivative, fnIt is the specific force vector for projecting to navigational coordinate system；wgAnd waIt is sensing Device noise；τbgAnd τbaRepresent the correlation time of inertial navigation noise；wbgAnd wbaIt is driving noise, symbol "×" indicates multiplication cross.
Further, the step (3) includes:
(31) the position detection equation of PDR or VLC output
When effective position information can not be exported by blocking VLC there is a situation where optical signal, PDR location information is fixed as appearance is surveyed The input quantity of position single Kalman filter module；The observational equation established according to PDR location information are as follows:
WhereinWithThe latitude and longitude calculated for INS mechanization；WithRespectively from PDR's Latitude and longitude；And nλIt is measurement noise；
Otherwise, it surveys appearance positioning single Kalman filter module and receives VLC location information, and ignore PDR location information；According to The observational equation that VLC output information is established are as follows:
Wherein,WithINS mechanization and the position vector of VLC are come from respectively；δrnIt is location error vector； n1It is to measure noise；
(32) magnetometer observational equation
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 vector of calibration, n3It is noise.
Further, the step (4) includes:
(41) by system modelIt is carried out being transformed into following form according to the model form of Kalman filtering:
Wherein,
(42) observation model Z=HX+V corresponds to following equation:
Wherein,H=[I3×3 03×3 03×3 03×3 03×3], V=[n1]；
When VLC exports inoperative position information, the location information inputted using PDR, observational equation are as follows:
When using VLC location information, observational equation are as follows:
When using the filtering of magnetometer observational equation, magnetometer observational equation is filtered according to participation after Z=HX+V formal argument Wave, whereinH=[03×3 03×3[mn×]03×3 03×3],
(43) steps are as follows for Extended Kalman filter recursion:
(a) initial value is set
Enable the initial value of covariance matrix P are as follows:
P0=diag ([var (r0) var(v0) var(ψ0) var(bg0) var(ba0)])
Wherein, r0、v0、ψ0、bg0And ba0Respectively indicate vector δ rn、δvn、ψ、bgAnd baInitial value, P0Indicate each vector The diagonal matrix that the covariance of initial vector is constituted；
If quantity of state X initial value is 0 vector；
(b) diagonal matrix at k moment is predicted using the diagonal matrix at k-1 moment, predictor formula are as follows:
Pk'=FPk-1FT+Q
Wherein, Pk-1Indicate 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 are as follows:
Wherein,Indicate the quantity of state at k-1 moment,Indicate the predicted value of k moment quantity of state；
(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) using the optimal value of the quantity of state at the kalman gain estimation k moment at k moment, enable the optimal value as the k moment Quantity of stateThen:
(f) using the optimal value of the diagonal matrix at the kalman gain estimation k moment at k moment, when enabling the optimal value as k The diagonal matrix P at quarterk, then:
Pk=(I-KkH)Pk′；
(g) posture information exported in k moment quantity of state feeds back to VLC locating module or PDR locating module, when exporting k It carves location information, velocity information and posture information in quantity of state and feeds back to INS module, export gyroscope in k moment quantity of state Bias vector feeds back to gyroscope, exports accelerometer bias vector in k moment quantity of state and feeds back to accelerometer；And export k The location information of receiver in moment quantity of state；Meanwhile k=k+1 is enabled, return step (b) continues cycling through filtering and constantly updates shape State amount exports location information.
The utility model has the advantages that compared with prior art, the single deck tape-recorder Germania of the invention based on MEMS sensor and VLC positioning fusion Filtering navigation device and method have the advantage that 1) surveying appearance using fusion in VLC positioning field for the first time accurately estimates that VLC is received The posture information of device, and eliminate the influence that posture positions VLC；2) it is discontinuous, rough scarce that VLC positioning result can be made up Point；3) positioning result can be provided using MEMS sensor information in the case where VLC signal is blocked, and efficiency of algorithm is high, at This is low；
Detailed description of the invention
Fig. 1 is to survey appearance positioning single Kalman filter navigation device structural schematic diagram；
Fig. 2 is 0 ° of pitch angle of emulation positioning result CDF curve；
Fig. 3 is 5 ° of pitch angle of emulation positioning result CDF curve；
Fig. 4 is 8 ° of pitch angle of emulation positioning result CDF curve；
Fig. 5 be receiving device lay flat, tilt 5 °, inclination 8 ° when analysis of Positioning Error result.
Specific embodiment
Technical solution of the present invention is described in detail with reference to the accompanying drawing.
As shown in Figure 1, the survey appearance based on MEMS sensor and VLC positioning fusion positions single Kalman filter navigation device It include: 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 the direction XYZ passes to PDR locating module all the way and carries out position Estimation, another way pass to INS module, and angular velocity signal of the receiver of gyroscope measurement in the direction XYZ passes to INS machinery Orchestration module.INS module carries out mechanization to acceleration signal and angular velocity signal by mechanization algorithm, obtains INS Location information, velocity information and posture information.PDR locating module carries out location estimation to acceleration signal, obtains the position PDR letter Breath.For magnetometer measures receiver with respect to the angle information in east, south, west, north direction, VLC locating module measures the position of receiver Information.
Although respective positioning result is not in addition, VLC locating module and PDR locating module are positioned simultaneously Equivalence, which is input to, to be surveyed in appearance positioning single Kalman filter module.Under normal circumstances, it surveys appearance and positions single Kalman filter module Mainly receive the location information of VLC locating module, and ignores the location information of PDR locating module.Only when generation optical signal hides When the case where VLC locating modules such as gear can not export effective position information, the location information of PDR module can just be used as main defeated Enter amount.
That is, the location information of the location information of PDR locating module and VLC locating module is passed through the position after weighted average Information is input to as unique location information surveys appearance positioning single Kalman filter module, when optical signal in VLC locating module The weight of the location information of VLC locating module is set as 0 by system when being blocked, only with the location information of PDR module.
The angle information and INS position information, velocity information that PDR location information, VLC location information, magnetometer measure It is input to posture information together as input quantity and surveys appearance positioning single Kalman filter module, worked as by filter process output Location information, velocity information and the posture information of preceding reception device feed back to INS module, export the appearance of current time receiver State information feeds back to PDR module or VLC locating module, 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.
It surveys appearance positioning and surveys appearance positioning single Kalman filter (Single Extended Kalman Filter, S-EKF) Focus on the modeling of state vector, system equation, state equation.S-EKF is mainly used to estimate the posture information of receiver (that is: roll angle, pitch angle and azimuth angle) and location information (that is: longitude, latitude and height).
A kind of air navigation aid based on above-mentioned navigation device, comprising the following steps:
(1) state vector of S-EKF is established
The state vector of S-EKF is defined as:
X=[δ rn δvn ψ bg ba]T (1)
Wherein, δ rn、δvn、ψ、bgAnd baIt is location error vector, the velocity error vector, attitude error of receiver respectively Vector, gyroscope bias vector and accelerometer bias vector.
(2) system model of S-EKF is established
(21) accelerometer and the collected receiver data information input of gyroscope are subjected to mechanization to INS module Algorithm process obtains receiver current location information, velocity information and posture information and is input in S-EKF；To attitude matrix into Row coordinate transform, coordinate equation of transfer are as follows:
Wherein,For rnFirst derivative,It is the position vector in navigational coordinate system,Indicate latitude, λ indicates that longitude and h indicate height；It is vnFirst derivative, vnIt is three-dimensional velocity vector；gnIt is the gravity in navigational coordinate system Vector；Indicate positional increment；Indicate speed increment；fbIt is the specific force vector in carrier coordinate system；D-1Be one aboutWith 3 × 3 matrixes of h；It isFirst derivative,It is to be predicted by INS mechanization slave carrier coordinate system to navigation seat Mark the direction cosine matrix of system；WithRespectively angular velocity vector WithSkew symmetry square Battle array；AndWithIndicate the angle of rotation speed of the angular speed and navigational coordinate system of earth autobiography relative to ECEF coordinate system Degree；WithIndicate carrier coordinate system relative to inertial coordinate rotational angular velocity and navigational coordinate system relative to inertial coordinate Rotational angular velocity.
(22) system model of A-EKF isSpecific expansion formula is as follows:
Wherein, δ symbol indicates error, the i.e. difference of true value and system nominal value；WithPoint It Biao Shi not δ rn、δvn、ψ、bgAnd baFirst derivative, fnIt is the specific force vector for projecting to navigational coordinate system；wgAnd waIt is sensing Device noise；τbgAnd τbaRepresent the correlation time of inertial navigation noise；wbgAnd wbaIt is driving noise, symbol "×" indicates multiplication cross.
(3) observational equation of S-EKF is established
The observational equation of S-EKF consists of two parts: 1) the position detection equation 2 of PDR or VLC output) magnetometer observation Equation.It should be noted that different sights should be used as input as input or VLC according to using PDR location information Survey equation.
(31) the position detection equation of PDR or VLC output
When effective position information can not be exported by blocking VLC there is a situation where optical signal, the location information of PDR is as survey appearance Position the main input quantity of single Kalman filter module；The observational equation established according to the position PDR are as follows:
WhereinWithThe latitude and longitude calculated for INS mechanization；WithLatitude and warp from PDR Degree；And nλIt is measurement noise.
Otherwise, the location information of appearance positioning single Kalman filter module primary recipient VLC is surveyed, and ignores the positioning of PDR Information；The observational equation established according to VLC output information are as follows:
Wherein,WithINS mechanization and the position vector of VLC are come from respectively；δrnIt is location error vector； n1It is to measure noise.
The weight of the PDR location information and VLC location information is set by whether optical signal in VLC locating module is blocked It is fixed, if the weight of VLC location information is set as 0 by system when optical signal is blocked, only with PDR location information.
(32) magnetometer observational equation
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 vector of calibration, n3It is noise.
(4) S-EKF is filtered
In order to further realize Extended Kalman filter, need system model (formula (3)) and observation model (formula (4)~(6)) it is updated to the time update equation and measurement updaue equation of Extended Kalman filter.It first first will system among the above Model and observation model are converted according to the model form of Kalman filtering, to correspond with the variable in model.
(41) by system model:The form can be converted by formula (3) come transformed equation such as Under:
Wherein,
(42) observation model: Z=HX+V, corresponding following equation:
Wherein,H=[I3×3 03×3 03×3 03×3 03×3], V=[n1]；
When VLC exports inoperative position information, the location information inputted using PDR, observational equation are as follows:
When using the location information of VLC, observational equation are as follows:
When using the filtering of magnetometer observational equation, also need to become magnetometer observational equation (6) according to Z=HX+V form Filtering is participated in after changing.Wherein,H=[03×3 03×3[mn×]03×3 03×3],
(43) Extended Kalman filter
After determining each variable, so that it may according to the time update equation of Extended Kalman filter and measurement updaue side Journey recursion is filtered.Steps are as follows for Extended Kalman filter recursion:
(a) initial value is set
Enable the initial value of covariance matrix P are as follows:
P0=diag ([var (r0) var(v0) var(ψ0) var(bg0) var(ba0)]) (11)
Wherein, r0、v0、ψ0、bg0And ba0Respectively indicate vector δ rn、δvn、ψ、bgAnd baInitial value, P0Indicate each vector The diagonal matrix that the covariance of initial vector is constituted.
If quantity of state X initial value is 0 vector.
(b) diagonal matrix at k moment is predicted using the diagonal matrix at k-1 moment, predictor formula are as follows:
Pk'=FPk-1FT+Q (12)
Wherein, Pk-1Indicate 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 are as follows:
Wherein,Indicate the quantity of state at k-1 moment,Indicate the predicted value of k moment quantity of state.
(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) using the optimal value of the quantity of state at the kalman gain estimation k moment at k moment, enable the optimal value as the k moment Quantity of stateThen:
(f) using the optimal value of the diagonal matrix at the kalman gain estimation k moment at k moment, when enabling the optimal value as k The diagonal matrix P at quarterk, then:
Pk=(I-KkH)Pk′ (16)
(g) posture information exported in k moment quantity of state feeds back to VLC locating module and PDR locating module, when exporting k It carves location information, velocity information and posture information in quantity of state and feeds back to INS module, export gyroscope in k moment quantity of state Bias vector feeds back to gyroscope, exports accelerometer bias vector in k moment quantity of state and feeds back to accelerometer；And export k The location information of receiver in moment quantity of state；
(h) k=k+1 is enabled, return step (b) continues cycling through filtering and constantly updates quantity of state, exports location information.
Wherein, step (b), the equation in (c) are time update equation, and (d), (e), equation is measurement updaue side in (f) Journey.
Filtering in addition to it is initial when carry out for (a)~(g), step (b)~(h) can be expressed as from recycling second Circulation carries out, and is continuously updated over time quantity of state, and the posture information in state can feed back to VLC locating module and PDR positioning Module, to correct the influence of posture, location information (location information and velocity information) can export outward.Since state vector is position Set 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 filter output, is accordingly changed into position and speed and is exported.
It is accurate that the fused filtering device (survey appearance and position single Kalman filter) surveys appearance using fusion in VLC positioning field for the first time Estimate the posture information of receiver, and eliminates the influence that posture positions VLC.Error equation conduct based on inertial navigation mechanization The system equation of fused filtering device.And observational equation then includes that accelerometer observed quantity updates (the position sight of PDR or VLC output Survey equation) and magnetometer observed quantity update.Fused filtering device exports the posture of receiver to VLC locating module to correct posture It influences.In the design of positioning filter, the location information of two-dimensional surface is based on pedestrian's dead reckoning as system mode vector Error equation as system equation, and the positioning result of VLC is then used as the observational equation of positioning filter.
Technical principle:
In VLC locating module, location algorithm generally uses three side location algorithms.Its principle is to utilize the more of indoor top Small cup lamp is as emission source, and generally at least three LED light, the light signal strength substitution optical signal measured in real time by receiver pass It broadcasts model and estimates target point to the distance of every lamp, estimate position location 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, In view of VLC system often has the random noises such as shot noise and thermal noise, often there is certain error in the estimation of distance, because The motion profile that this position estimated is formed can have the features such as precipitous variation, and the movement that this does not often meet realistic objective is special Sign.Since in target following, priori knowledge should indicate 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.
General VLC positioning often assumes that equipment is to lay flat posture at present, to simplify location algorithm.However in reality Equipment can generate 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 strength changes.If can not accurately estimate the posture of equipment, the light signal strength variation of this part will become system A part of error, causes position error to become larger.In order to illustrate attitude error for the influence degree of VLC positioning accuracy, we The precision that VLC is positioned in the case where by the different pitch angles of contrast simulation.The emulation experiment determines receiver run-off the straight The case where position algorithm does not correct is simulated, and simulates two different inclination conditions, positioning result CDF to all receivers Curve difference is as shown in Figure 3 and Figure 4, and Fig. 2 indicates the positioning result of the non-run-off the straight of equipment.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 comparison receiver is tilted and laid flat Fruit, as shown in Figure 5, it can be seen that when receiver is tilted, if be not modified to receiver posture, position error meeting It is significant to increase.This is simulation results show accurately the posture of estimation receiver has weight to the performance for improving visible light positioning system Want meaning.Therefore, MEMS Attitude estimation module is introduced, accurate posture information is passed into VLC locating module and carries out posture school Standard can significantly reduce influence of the posture to VLC positioning result.
Since VLC positioning can not carry out optical signal in the case where being blocked, introduces PDR locating module and 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, the motion profile of pedestrian can be obtained.Therefore, signal occurs when VLC is positioned When blocking, can not output position as a result, emerging system can be made up using the positioning result of PDR locating module.

Claims (7)

1. the single deck tape-recorder Kalman Filtering navigation device based on MEMS sensor and VLC positioning fusion, it is characterised in that: passed including MEMS Sensor, INS module, VLC locating module, PDR locating module and survey appearance position single Kalman filter module；The MEMS sensing Device includes accelerometer, gyroscope and magnetometer；
The input for surveying appearance positioning single Kalman filter module includes: the receiver that measures of accelerometer in the direction XYZ Angular velocity information of the receiver that acceleration information and gyroscope measure on the direction XYZ passes to INS module, by INS module Layout is carried out to it obtains INS position, speed and the posture information of receiver；The receiver that magnetometer measures relatively east, south, West, the north to angle information；The receiver that accelerometer measures the direction XYZ acceleration information to PDR locating module into The VLC location information for PDR location information and VLC the locating module output that row location estimation obtains passes through the position after weighted average Information；
Location information, velocity information and the posture for surveying appearance positioning single Kalman filter module output current time receiver Information gives INS module, exports the posture information of current time receiver to PDR locating module or VLC locating module, output noise Thermal compensation signal feeds back to accelerometer and gyroscope, while exporting the location information of current time receiver.
2. navigation device according to claim 1, it is characterised in that: the power of the PDR location information and VLC location information Weight is set by whether optical signal in VLC locating module is blocked, if system is by the power of VLC location information when optical signal is blocked It is reset to 0, only with PDR location information.
3. a kind of air navigation aid based on navigation device as claimed in claim 1 or 2, which comprises the following steps:
(1) state vector for surveying appearance positioning single Kalman filter module S-EKF is established；
(2) system model for surveying appearance positioning single Kalman filter module S-EKF is established；
(3) observational equation for surveying appearance positioning single Kalman filter module S-EKF is established；
(4) appearance positioning single Kalman filter module S-EKF filtering output location information is surveyed.
4. a kind of air navigation aid according to claim 3, it is characterised in that: the state vector of S-EKF in the step (1) Are as follows:
X=[δ rn δvn ψ bg ba]T
Wherein, δ rn、δvn、ψ、bgAnd baBe respectively the location 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, which is characterized in that the step (2) includes:
(21) accelerometer and the collected receiver data information input of gyroscope are subjected to mechanization algorithm to INS module Processing obtains receiver current location information, velocity information and posture information and is input in S-EKF；Attitude matrix is sat Mark transformation, coordinate equation of transfer are as follows:
Wherein,For rnFirst derivative,It is the position vector in navigational coordinate system,Indicate that latitude, λ indicate Longitude and h indicate height；It is vnFirst derivative, vnIt is three-dimensional velocity vector；gnIt is the gravity vector in navigational coordinate system；Indicate positional increment；Indicate speed increment；fbIt is the specific force vector in carrier coordinate system；D-1Be one aboutWith h's 3 × 3 matrixes；It isFirst derivative,It is the side slave carrier coordinate system to navigational coordinate system predicted by INS mechanization To cosine matrix；WithRespectively angular velocity vector WithSkew symmetric matrix；And WithIndicate the rotational angular velocity of the angular speed and navigational coordinate system of earth autobiography relative to ECEF coordinate system；With Indicate that carrier coordinate system is fast relative to the angle of rotation of inertial coordinate relative to the rotational angular velocity and navigational coordinate system of inertial coordinate Degree；
(22) system model of S-EKF isSpecific expansion formula is as follows:
Wherein, δ symbol indicates error, the i.e. difference of true value and system nominal value；WithIt respectively indicates δ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 "×" indicates multiplication cross.
6. a kind of air navigation aid according to claim 5, which is characterized in that the step (3) includes:
(31) the position detection equation of PDR or VLC output
When effective position information can not be exported by blocking VLC there is a situation where optical signal, PDR location information is single as appearance positioning is surveyed The input quantity of Kalman filter module；The observational equation established according to PDR location information are as follows:
WhereinWithThe latitude and longitude calculated for INS mechanization；WithRespectively latitude and warp from PDR Degree；And nλIt is measurement noise；
Otherwise, it surveys appearance positioning single Kalman filter module and receives VLC location information, and ignore PDR location information；According to VLC The observational equation that output information is established are as follows:
Wherein,WithINS mechanization and the position vector of VLC are come from respectively；δrnIt is location error vector；n1The amount of being Survey noise；
(32) magnetometer observational equation
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 vector of calibration, n3It is noise.
7. a kind of air navigation aid according to claim 6, which is characterized in that the step (4) includes:
(41) by system modelIt is carried out being transformed into following form according to the model form of Kalman filtering:
Wherein,
(42) observation model Z=HX+V corresponds to following equation:
Wherein,H=[I3×3 03×3 03×3 03×3 03×3], V=[n1]；
When VLC exports inoperative position information, the location information inputted using PDR, observational equation are as follows:
When using VLC location information, observational equation are as follows:
When using the filtering of magnetometer observational equation, magnetometer observational equation is filtered according to participation after Z=HX+V formal argument, In,H=[03×3 03×3 [mn×] 03×3 03×3],
(43) steps are as follows for Extended Kalman filter recursion:
(a) initial value is set
Enable the initial value of covariance matrix P are as follows:
P0=diag ([var (r0) var(v0) var(ψ0) var(bg0) var(ba0)])
Wherein, r0、v0、ψ0、bg0And ba0Respectively indicate vector δ rn、δvn、ψ、bgAnd baInitial value, P0Indicate that each vector is initial The diagonal matrix that the covariance of vector is constituted；
If quantity of state X initial value is 0 vector；
(b) diagonal matrix at k moment is predicted using the diagonal matrix at k-1 moment, predictor formula are as follows:
P′k=FPk-1FT+Q
Wherein, Pk-1Indicate the diagonal matrix at k-1 moment, P 'kIndicate the predicted value of the diagonal matrix at 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 are as follows:
Wherein,Indicate the quantity of state at k-1 moment,Indicate the predicted value of k moment quantity of state；
(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 'kIndicate the kalman gain at k moment, R=E [VVT]；
(e) using the optimal value of the quantity of state at the kalman gain estimation k moment at k moment, shape of the optimal value as the k moment is enabled State amountThen:
(f) using the optimal value of the diagonal matrix at the kalman gain estimation k moment at k moment, enable the optimal value as the k moment Diagonal matrix Pk, then:
Pk=(I-KkH)P′k；
(g) posture information exported in k moment quantity of state feeds back to VLC locating module or PDR locating module, exports k moment shape Location information, velocity information and posture information in state amount feed back to INS module, export gyroscope deviation in k moment quantity of state Vector feedback exports accelerometer bias vector in k moment quantity of state 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 enabled, return step (b) continues cycling through filtering and constantly updates quantity of state, Export location information.
CN201710505803.6A 2017-06-28 2017-06-28 Single deck tape-recorder Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion Active CN107289932B (en)

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 CN107289932A (en) 2017-10-24
CN107289932B true 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)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
CN110031803B (en) * 2019-04-04 2020-11-27 中国科学院数学与系统科学研究院 Fusion positioning method of double infrared sensors with random measurement noise
CN110132257B (en) * 2019-05-15 2023-03-24 吉林大学 Human behavior prediction method based on multi-sensor data fusion
CN111811500A (en) * 2020-05-06 2020-10-23 北京嘀嘀无限科技发展有限公司 Target object pose estimation method and device, storage medium and electronic equipment
CN114370870B (en) * 2022-01-05 2024-04-12 中国兵器工业计算机应用技术研究所 Filter update information screening method suitable for pose measurement Kalman filtering

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103175529B (en) * 2013-03-01 2016-01-06 上海美迪索科电子科技有限公司 Based on pedestrian's inertial positioning system that indoor magnetic signature is auxiliary
KR20170060034A (en) * 2014-09-08 2017-05-31 인벤센스, 인크. 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)
CN105222772B (en) * 2015-09-17 2018-03-16 泉州装备制造研究所 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
CN106225784B (en) * 2016-06-13 2019-02-05 国家海洋局第二海洋研究所 Based on inexpensive Multi-sensor Fusion pedestrian dead reckoning method

Also Published As

Publication number Publication date
CN107289932A (en) 2017-10-24

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
CN105628026B (en) A kind of positioning and orientation method and system of mobile object
CN107246872B (en) Single-particle filtering navigation device and method based on MEMS sensor and VLC positioning fusion
CN104729506B (en) A kind of unmanned plane Camera calibration method of visual information auxiliary
CN110243358A (en) The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion
CN109556632A (en) INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering
CN105606094B (en) A kind of information condition matched filtering method of estimation based on MEMS/GPS combined systems
CN110779521A (en) Multi-source fusion high-precision positioning method and device
CN110645979A (en) Indoor and outdoor seamless positioning method based on GNSS/INS/UWB combination
CN108873038A (en) Autonomous parking localization method and positioning system
CN110398257A (en) The quick initial alignment on moving base method of SINS system of GPS auxiliary
CN108226980A (en) Difference GNSS and the adaptive close coupling air navigation aids of INS based on Inertial Measurement Unit
CN111426320B (en) Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter
CN107702712A (en) Indoor pedestrian's combined positioning method based on inertia measurement bilayer WLAN fingerprint bases
CN110398245A (en) The indoor pedestrian navigation Attitude estimation method of formula Inertial Measurement Unit is worn based on foot
CN108387227A (en) The multinode information fusion method and system of airborne distribution POS
CN108680942A (en) A kind of inertia/multiple antennas GNSS Combinated navigation methods and device
CN108562917A (en) The constraint filtering of additional orthogonal Function Fitting condition resolves method and device
CN104251702A (en) Pedestrian navigation method based on relative pose measurement
CN108871325B (en) A kind of WiFi/MEMS combination indoor orientation method based on two layers of Extended Kalman filter
CN111025366A (en) Grid SLAM navigation system and method based on INS and GNSS
CN109931952A (en) The direct analytic expression coarse alignment method of inertial navigation under the conditions of unknown latitude
CN112729301A (en) Indoor positioning method based on multi-source data fusion

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