CN109959918B - Solid body positioning method and device and computer storage medium - Google Patents

Solid body positioning method and device and computer storage medium Download PDF

Info

Publication number
CN109959918B
CN109959918B CN201910134391.9A CN201910134391A CN109959918B CN 109959918 B CN109959918 B CN 109959918B CN 201910134391 A CN201910134391 A CN 201910134391A CN 109959918 B CN109959918 B CN 109959918B
Authority
CN
China
Prior art keywords
solid body
estimator
estimate
anchor node
information
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
CN201910134391.9A
Other languages
Chinese (zh)
Other versions
CN109959918A (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.)
Xidian University
Original Assignee
Xidian 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 Xidian University filed Critical Xidian University
Priority to CN201910134391.9A priority Critical patent/CN109959918B/en
Publication of CN109959918A publication Critical patent/CN109959918A/en
Application granted granted Critical
Publication of CN109959918B publication Critical patent/CN109959918B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The embodiment of the invention discloses a method and a device for positioning a solid body and a computer storage medium; the method can comprise the following steps: determining direction information and position information of a target solid body based on sensors distributed on the target solid body; acquiring noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein the anchor nodes comprise anchor nodes having a position error; constructing an estimator according to the direction information and the position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and acquiring a log-likelihood function of the estimator; and based on the log-likelihood function of the estimator, acquiring a maximum likelihood solution of the estimator according to a set maximum likelihood estimation algorithm strategy.

Description

Solid body positioning method and device and computer storage medium
Technical Field
The present invention relates to the field of signal processing technologies, and in particular, to a method and an apparatus for positioning a solid body, and a computer storage medium.
Background
Active positioning technology has been the focus of attention in the industry because of its effectiveness, and includes sonar positioning, radar positioning, microphone array positioning, and currently available Wireless Sensor Networks (WSNs) positioning. Conventional active localization techniques typically treat the object of interest as a point source, such that the location of the object can be defined by a three-dimensional (3D) single point location or a two-dimensional (2D) location area. However, in many positioning applications, the object of interest needs to be treated as a solid body with a deformation of zero or so small that it is negligible; and on a solid body, the distance between any given two points is not negligible; thus, the solid Body Localization (RBL) technique has emerged.
In conventional RBL techniques, a stationary solid body can be precisely located using time or distance measurement methods between sensors disposed in the solid body and some surrounding landmark information (also referred to as anchor nodes); therefore, accurate position estimation of the solid body is highly dependent on the position accuracy of the anchor node. However, in general, the position error of the anchor node causes the reduction of the positioning accuracy of the solid body, and therefore, it is necessary to accurately estimate the position of the solid body for the erroneous anchor node.
Disclosure of Invention
In view of the above, embodiments of the present invention are directed to a method, an apparatus, and a computer storage medium for solid body positioning; and under the condition that the position of the anchor node has an error, the position of the solid body can be accurately estimated.
The technical scheme of the invention is realized as follows:
in a first aspect, an embodiment of the present invention provides a method for positioning a solid body, where the method includes:
determining direction information and position information of a target solid body based on sensors distributed on the target solid body;
acquiring noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein the anchor nodes comprise anchor nodes having a position error;
constructing an estimator according to the direction information and the position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and acquiring a log-likelihood function of the estimator; wherein the estimates comprise a direction estimate and a location estimate of the target solid body, and a location estimate of the anchor node;
based on the log-likelihood function of the estimator, acquiring a maximum likelihood solution of the estimator according to a set maximum likelihood estimation algorithm strategy; wherein the maximum likelihood solution for the estimator comprises a direction estimate and a position estimate for the target solid state volume, and a position estimate for the anchor node.
In a second aspect, an embodiment of the present invention provides an apparatus for positioning a solid body, the apparatus including: a determination section, an acquisition section, a construction section and an estimation section; wherein,
the determination section is configured to determine direction information and position information of a target solid body based on sensors distributed on the target solid body;
the acquisition part is configured to acquire noise covariance information of anchor nodes distributed around the target solid body and a measurement vector; wherein the anchor nodes comprise anchor nodes having a position error;
the constructing part is configured to construct an estimator according to the direction information and the position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and acquire a log-likelihood function of the estimator; wherein the estimates comprise a direction estimate and a location estimate of the target solid body, and a location estimate of the anchor node;
the estimation part is configured to obtain a maximum likelihood solution of the estimator according to a set maximum likelihood estimation algorithm strategy based on a log-likelihood function of the estimator; wherein the maximum likelihood solution for the estimator comprises a direction estimate and a position estimate for the target solid state volume, and a position estimate for the anchor node.
In a third aspect, an embodiment of the present invention provides an apparatus for positioning a solid body, where the apparatus includes: a communication interface, a memory and a processor; the communication interface is used for receiving and sending signals in the process of receiving and sending information with other external network elements;
the memory for storing a computer program operable on the processor;
the processor is configured to perform the method steps of the solid body positioning of the first aspect when running the computer program.
In a fourth aspect, an embodiment of the present invention provides a computer storage medium, where the computer storage medium stores a program for solid body positioning, and the program for solid body positioning is executed by at least one processor to implement the method steps of solid body positioning according to the first aspect.
The embodiment of the invention provides a method and a device for positioning a solid body and a computer storage medium; the direction and the position of the target solid body and the position of the anchor node with the position error are jointly estimated through a maximum likelihood estimation algorithm, so that the problem of nonlinear constraint optimization existing in joint estimation is solved. But also can accurately estimate the position of the solid body under the condition that the position of the anchor node has errors.
Drawings
Fig. 1 is a schematic flow chart of a method for positioning a solid body according to an embodiment of the present invention;
fig. 2 is a schematic diagram of correction performance of an anchor node position a according to an embodiment of the present invention;
fig. 3 is a schematic diagram illustrating an influence of the estimation accuracy of the rotation angle q according to the embodiment of the present invention;
fig. 4 is a schematic diagram illustrating an influence of the estimation accuracy of the translation vector t according to the embodiment of the present invention;
fig. 5 is a schematic diagram illustrating an estimation effect of an anchor node position a according to an embodiment of the present invention;
fig. 6 is a schematic diagram illustrating an estimation effect of the rotation angle q according to the embodiment of the present invention;
fig. 7 is a schematic diagram illustrating an estimation effect of a translation vector t according to an embodiment of the present invention;
FIG. 8 is a schematic diagram of a solid positioning device according to an embodiment of the present invention;
FIG. 9 is a schematic diagram of another solid positioning device according to an embodiment of the present invention;
fig. 10 is a schematic hardware structure diagram of a solid positioning device according to an embodiment of the present invention.
Detailed Description
The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.
Solid body positioning is currently widely applicable in unmanned vehicles, robotic systems, underwater vehicles, ships, drones, orbiting satellites, spacecraft and many other fields where it is desirable to not only estimate the absolute position of a solid body, but also to determine its direction accurately, which can be called direction finding in the fields of aerospace, marine heading, unmanned vehicle direction, and robotic systems or industrial equipment tilt and consumer equipment. Taking an unmanned vehicle as an example, the unmanned vehicle as a solid body not only moves but also turns, and therefore determining the direction of the solid body is necessary for basic control, steering and safety assurance. In conventional approaches to solid body positioning, although the position and orientation are closely related, they are still treated separately.
For example, some conventional schemes are to install several sensors on a solid body, and to jointly determine their direction and location using range measurements (or time measurements) for a wireless sensor network (which, in embodiments of the invention, may be composed of some landmarks simply referred to as anchor nodes). While the absolute position of the solid body is unknown when the solid body is manufactured, the topology of the sensors mounted on the solid body or the relative position of the sensors can be measured, and thus the direction and absolute position of the solid body can be estimated. For a solid body, the direction of the solid body is represented as a rotation matrix (or rotation angle vector) and the absolute position of the solid body is represented as a translation vector. And jointly estimating the rotation matrix (or rotation angle vector) and the translation vector can be considered as a non-linear constraint optimization problem because the rotation matrix (or rotation angle vector) and the translation vector are non-linearly related to the distance measurement and are closely related to each other. Furthermore, the rotation matrix is not a free variable, it must belong to a special orthogonal group, which means that its elements must satisfy a quadratic constraint in the two-dimensional case and a cubic constraint in the three-dimensional case. In view of the above problems, embodiments of the present invention are expected to be able to solve by Maximum Likelihood Estimation (MLE).
Based on this, referring to fig. 1, a method for positioning a solid body according to an embodiment of the present invention is shown, which may include:
s101: determining direction information and position information of a target solid body based on sensors distributed on the target solid body;
s102: acquiring noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein the anchor nodes comprise anchor nodes having a position error;
s103: constructing an estimator according to the direction information and the position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and acquiring a log-likelihood function of the estimator; wherein the estimates comprise a direction estimate and a location estimate of the target solid body, and a location estimate of the anchor node;
s104: based on the log-likelihood function of the estimator, acquiring a maximum likelihood solution of the estimator according to a set maximum likelihood estimation algorithm strategy; wherein the maximum likelihood solution for the estimator comprises a direction estimate and a position estimate for the target solid state volume, and a position estimate for the anchor node.
By the technical scheme shown in fig. 1, the direction and the position of the target solid body and the position of the anchor node with the position error are jointly estimated through the maximum likelihood estimation algorithm, so that the problem of nonlinear constraint optimization in joint estimation is solved. But also can accurately estimate the position of the solid body under the condition that the position of the anchor node has errors.
With respect to the technical solution shown in fig. 1, in a possible implementation manner, the determining direction information and position information of a target solid body based on sensors distributed on the target solid body includes:
corresponding to the number of the sensors being N and the relative positions of the sensors being known, the position information of the sensors under the local reference frame is
Figure GDA0003290609730000051
Wherein N represents a sensor identifier, N is 1,2, …, N;
determining the position information of each sensor under the global reference system based on the position information of each sensor under the local reference system
Figure GDA0003290609730000052
Wherein,
Figure GDA0003290609730000053
q and t are respectively a rotation matrix and a translation vector from a local reference frame to a global reference frame and are used for representing the direction information and the position information of the target solid body.
For the technical solution shown in fig. 1, in a possible implementation manner, the acquiring noise covariance information of anchor nodes distributed around the target solid body and a measurement vector includes:
corresponding to the number of the anchor nodes being M, the position information of the anchor nodes under the global reference frame is amWherein M represents an anchor node identifier, M is 1,2, …, M;
generating a position matrix of the anchor node according to the position information of the anchor node
Figure GDA0003290609730000054
A noise covariance matrix of the anchor node corresponding to a zero-mean and independently identically distributed Gaussian random process of the position error of the anchor node
Figure GDA0003290609730000055
Wherein, the sigmaaIs the standard deviation sigma of the position error of the anchor nodea(m)；
Corresponding to the measurement vector
Figure GDA0003290609730000061
Is Gaussian distributed and the covariance matrix of the measurement vectors is Rr,nAll measurement vectors
Figure GDA0003290609730000062
Is Gaussian distributed and has a covariance matrix of all measurement vectors of Rr＝Bdiag(Rr,1,Rr,2,…,Rr,N)。
It should be noted that the above implementation may be used to set a positioning scenario of a target solid body, and specifically, when setting a positioning scenario, N sensor nodes may be distributed on a static target solid body, M anchor nodes with position errors may be distributed around the target solid body, and the position errors of the anchor nodes may be regarded as a zero-mean independent and uniformly distributed gaussian random process, and a standard deviation σ of the position errors of each anchor nodea(m)＝σaWith a covariance matrix of
Figure GDA0003290609730000063
For the measurement vectors, the number of the measurement vectors is consistent with the number of sensors, the number of the measurement elements in each measurement vector is the same as the number of anchor nodes, the rigid body sensor array is usually smaller and far away from the anchor nodes, and therefore the distance measurement noise power of different sensors is the same, namely Rr,1＝Rr,2＝…＝Rr,N。
For the foregoing technical solution, in a possible implementation manner, the constructing an estimator according to the direction information and the position information of the target solid, the noise covariance information of the anchor node, and the measurement vector, and acquiring a log likelihood function of the estimator includes:
generating an estimate
Figure GDA0003290609730000064
Wherein,
Figure GDA0003290609730000065
qoand toRepresenting the rotation angle estimate and the translation vector estimate, A, of the target solid body, respectivelyoA position information estimator representing the anchor node, the rotation angle Q of the target solid body having the following relationship to the target solid body rotation matrix Q:
q＝[α,β,γ]T，Q＝QγQβQα，
Figure GDA0003290609730000066
Figure GDA0003290609730000067
wherein the target solid rotation matrix Q is a special orthogonal group
Figure GDA0003290609730000068
According to the estimated quantity psioDetermining a log-likelihood function of the estimator as:
Figure GDA0003290609730000071
wherein, eta is a constant value,
Figure GDA0003290609730000072
for the foregoing technical solution, in a possible implementation manner, the obtaining a maximum likelihood solution of the estimator according to a set maximum likelihood estimation algorithm policy based on a log-likelihood function of the estimator includes:
log-likelihood function lnp (ζ, ψ) according to maximum likelihood rule based on the estimatoro) Generating pairsThe corresponding least squares model is as follows:
Figure GDA0003290609730000073
set initial value ζo(ψ)＝ζo(ψ{0})+G{0}(ψ-ψ{0}) Wherein G is{0}Is a gradient matrix and
Figure GDA0003290609730000074
wherein,
Figure GDA0003290609730000075
Figure GDA0003290609730000076
wherein,
Figure GDA0003290609730000077
Figure GDA0003290609730000078
Figure GDA0003290609730000079
is QoRelative to qoA derivative of (a);
based on the initial value, iteration is carried out according to the set iteration times and an iteration equation shown in the following formula for the least square model according to a Gauss-Newton iteration method, and the maximum likelihood solution of the estimator is obtained
Figure GDA00032906097300000710
ψ{k+1}＝ψ{k}+(G{k}TR-1G{k})-1G{k}TR-1(ζ-ζo(ψ{k})),k＝0,1,...
Where k represents the number of iterations.
In addition, according to the settingThe iterative solution obtained after iteration is carried out for the iteration times tends to converge so as to meet the set precision requirement, and at the moment, the obtained iterative solution is regarded as the maximum likelihood solution of the estimator
Figure GDA0003290609730000081
It is understood that by modeling the anchor node position error as additive gaussian noise, in the case where the anchor node precise position is not completely clear, it can be derived from Cramer-Rao Lower Bound (CRLB) analysis: the reduction of the positioning accuracy of the target solid body is caused by the position error of the anchor node; so that it can be known that: in a more practical application scenario, the accurate position estimation of the target solid-state body is highly dependent on the position accuracy of the anchor node. The technical solution and implementation shown in fig. 1 above may determine the technical effect of the method for solid body positioning proposed by the embodiment of the present invention through comparison with the lower boundary of cramer-circle, based on a Maximum Likelihood Estimation (MLE) method proposed for the position error of the anchor node.
Based on this, for the above technical solution and implementation shown in fig. 1, in a possible implementation, the method further includes:
determining a cramer-circle lower bound of a rotation angle estimator and a translation vector estimator of the target solid body from the estimator and a log-likelihood function of the estimator;
the mean square error of the maximum likelihood solution of the estimator is obtained under a plurality of anchor node position error strengths and compared with the lower clalmelo bound of each anchor node position error strength.
Preferably, for the above implementation, the determining a cramer-circle lower bound of the rotation angle estimate and the translation vector estimate of the target solid body from the estimate and a log-likelihood function of the estimate comprises:
log likelihood function lnp (ζ, ψ) of the estimatoro) For the estimation
Figure GDA0003290609730000082
Calculating the second partial derivative to obtain a Fisher Information matrix
Figure GDA0003290609730000083
Wherein,
Figure GDA0003290609730000084
Figure GDA0003290609730000085
Figure GDA0003290609730000086
wherein,
Figure GDA0003290609730000087
Figure GDA0003290609730000088
Figure GDA0003290609730000089
is QoRelative to qoA derivative of (a);
FIM (psi) based on the snow information matrixo) Respectively obtaining the estimators psi by a block matrix inversion formulaoIn
Figure GDA0003290609730000091
Lower boundary of Cramer-O
Figure GDA0003290609730000092
And A ofoCrLB (A) of the lower CrLB boundaryo)＝FIM(Ao)-1＝(Z-YTX-1Y)-1。
It should be noted that the technical effect of the method for positioning a solid body provided in the embodiment of the present invention can be intuitively obtained by comparing the mean square error of the maximum likelihood solution of the estimators under various anchor node position error strengths with the lower clarmero bound of each anchor node position error strength. The embodiment of the invention explains the technical effect of the method for positioning the solid body provided by the embodiment of the invention through the following specific simulation scene.
First, simulation conditions are set as follows:
1. based on a 3D positioning scene, under the condition that the positions of anchor nodes are uncertain, the number of the anchor nodes is set to be 6, and the anchor nodes are uniformly distributed in a three-dimensional cube (plus or minus 50 mxplus 50M) taking an origin O as a center in a global reference system. In order to avoid that poor distribution structures influence the positioning performance, the distance between two anchor nodes is at least 15 meters. According to the above conditions, 200 geometric distributions of M-6 anchor nodes can be randomly generated, and the simulation result is the average value thereof. For each given geometry, the number of simulation runs is L1000, and each sensor is able to obtain measurements from all anchor nodes.
2. The target solid body settings and sensor configuration are specified as follows: for a 3D RBL scenario, the true position of the rigid body sensor is
Figure GDA0003290609730000093
In the local reference frame, each column in C represents a sensor position Cn. The rotation of the target solid body is set as follows: the two reference frames are initially coincident and then the solid body is rotated α -20 degrees, β -25 degrees and γ -10 degrees. The translation vector is t ═ 100,50]T. Throughout the simulation, the solid body sensor array is typically small and far from the anchor node. Thus, the noise power in the range from a given anchor node to all sensors can be set to be the same. But for a given rigid body sensor, the noise power from the rigid body sensor to different anchor point ranges may be set to be different in order to better exploit the performance of the algorithm.
Thus, for a number of anchor nodes M-6, the covariance matrix of the distance measurement noise is
Figure GDA0003290609730000101
Wherein,
Figure GDA0003290609730000102
is kronecker product. In subsequent simulations, the distance measurement noise power in a 3D scene
Figure GDA0003290609730000103
Fixed at-40 dB. Adding a covariance matrix of
Figure GDA0003290609730000104
The true value of the zero mean gaussian noise to construct the anchor node position with the position error. The settings of the noise covariance matrix were used for all numerical examples and simulations in this simulation.
Next, simulation is performed based on the simulation conditions and the method for positioning the solid body, and for a specific process, reference is made to the technical scheme and each implementation manner shown in fig. 1, which are not described herein again. The specific simulation results and analysis are as follows:
1. CRLB assay
Noise power by changing anchor node location
Figure GDA0003290609730000105
The proposed CRLB result may be used to verify the effect of anchor node location a on solid body positioning. For the 3D scene, CRLB simulation results are shown in fig. 2 to 4. Fig. 2 shows correction performance of the anchor node position a, fig. 3 shows influence of estimation accuracy of the rotation angle q, and fig. 4 shows influence of estimation accuracy of the translation vector t. In fig. 3 and 4, CRLB of parameter estimation in the absence of anchor node position error Δ a is also plotted for comparison.
For fig. 2, the abscissa is the Noise power (Anchor Position Noise) of the Anchor node Position, the ordinate is the root value of the CRLB of the Anchor node Position a, the circled dashed line represents the CRLB boundary without Position correction, and the dotted line represents the CRLB boundary with Position correction, so that it can be seen that, after correction is performed using the distance measurement vector r, when the Noise intensity of the Anchor node Position is relatively large, the estimated CRLB boundary of the Anchor node Position a is significantly smaller than that without any correction, which can prove that when the measurement vector r is used to estimate the target solid stateBody position vector
Figure GDA0003290609730000106
The anchor node position uncertainty Δ a can be corrected significantly.
For fig. 3 and 4, the abscissa thereof is the Noise power (Anchor Position Noise) at the Anchor node Position, while the ordinate of fig. 3 is the root value of CRLB at the rotation angle q of the solid body, and the ordinate of fig. 4 represents the root value of CRLB at the translation vector t of the solid body.
The meter-shaped chain line represents the CRLB boundary of the parameter estimation in the absence of the anchor node position error Δ a, and the dotted square line represents the CRLB boundary in the presence of the anchor node position error Δ a, so that it can be seen that the CRLB estimated by q and t is obviously determined by the anchor node position in the presence of the anchor node position error Δ a, which proves that the estimation performance of q and t is obviously deteriorated by the anchor node position uncertainty Δ a. This means that even the distance measurement noise power
Figure GDA0003290609730000111
At a small level, the q and t estimation errors caused by the anchor node position error Δ a are not negligible in practical scenario applications.
2. Analysis for Root Mean Square Error (RMSE)
Fig. 5 shows the estimation effect of the anchor node position a of the proposed solution, fig. 6 and 7 show the estimation effect of the rotation angle q and the translation vector t, respectively, and in fig. 6 and 7, the CRLB of the parameter estimation in the absence of the anchor node position error Δ a is also plotted for comparison.
For fig. 5, the abscissa is the Noise power (Anchor Position Noise) of the Anchor node Position, the ordinate is the RMSE of the Anchor node Position a, the meter-shaped dot-dash line represents the RMSE of the estimated value obtained by using the solid body positioning method provided by the embodiment of the present invention, and the square dot-dash line represents the CRLB boundary under the condition that the Anchor node has a Position error, so it can be seen that when the Noise power (Anchor Position Noise) of the Anchor node Position is present
Figure GDA0003290609730000112
From 10-4Increased to 101In the process, the mean square error (RMSE) of the estimation of the anchor node position a by the solid body positioning method provided by the embodiment of the invention can well realize the corresponding CRLB precision.
For fig. 6 and 7, the abscissa is the Noise power at the Anchor node Position (Anchor Position Noise), while the ordinate of fig. 6 is the RMSE of the rotation angle q of the solid body, and the ordinate of fig. 7 represents the RMSE of the translation vector t of the solid body.
The metric dot-dash line represents RMSE of the estimated value obtained by using the solid body positioning method proposed in the embodiment of the present invention, the dotted square line represents the CRLB boundary when the anchor node position error Δ a exists, and the solid circular line represents the CRLB boundary when the anchor node position error Δ a does not exist. Therefore, when the noise power of the anchor node position is low, the solid body positioning method provided by the embodiment of the invention can well realize the corresponding CRLB precision for the mean square error (RMSE) of the estimation of the translation vector t and the rotation angle q; when the noise power reaches 10 at the anchor node position1The mean square error (RMSE) of the rotation angle q estimate is slightly above its cramer limit, while the Mean Square Error (MSE) of the translation vector t estimate is significantly above its cramer limit. From this it can be seen that: the influence of the anchor node position error Δ a on the translation vector t is more severe than the rotation angle q, so the influence of the anchor node error must be considered in practical application.
Based on the same inventive concept of the foregoing technical solution, referring to fig. 8, an apparatus 80 for positioning a solid body according to an embodiment of the present invention is shown, where the apparatus 80 may include: determination section 801, acquisition section 802, construction section 803, and estimation section 804; wherein,
the determination section 801 is configured to determine direction information and position information of a target solid body based on sensors distributed on the target solid body;
the acquisition section 802 configured to acquire noise covariance information of anchor nodes distributed around the target solid body and a measurement vector; wherein the anchor nodes comprise anchor nodes having a position error;
the constructing section 803 is configured to construct an estimator from the direction information and the position information of the target solid body and the noise covariance information of the anchor node and the measurement vector, and acquire a log-likelihood function of the estimator; wherein the estimates comprise a direction estimate and a location estimate of the target solid body, and a location estimate of the anchor node;
the estimating part 804 is configured to obtain a maximum likelihood solution of the estimator according to a set maximum likelihood estimation algorithm strategy based on a log-likelihood function of the estimator; wherein the maximum likelihood solution for the estimator comprises a direction estimate and a position estimate for the target solid state volume, and a position estimate for the anchor node.
In the above scheme, the determining section 801 is configured to:
corresponding to the number of the sensors being N and the relative positions of the sensors being known, the position information of the sensors under the local reference frame is
Figure GDA0003290609730000121
Wherein N represents a sensor identifier, N is 1,2, …, N;
determining the position information of each sensor under the global reference system based on the position information of each sensor under the local reference system
Figure GDA0003290609730000122
Wherein,
Figure GDA0003290609730000123
q and t are respectively a rotation matrix and a translation vector from a local reference frame to a global reference frame and are used for representing the direction information and the position information of the target solid body.
In the above scheme, the obtaining part 802 is configured to:
corresponding to the number of the anchor nodes being M, the position information of the anchor nodes under the global reference frame is amWherein M represents an anchor node identifier, M is 1,2, …, M;
generating a position matrix of the anchor node according to the position information of the anchor node
Figure GDA0003290609730000124
A noise covariance matrix of the anchor node corresponding to a zero-mean and independently identically distributed Gaussian random process of the position error of the anchor node
Figure GDA0003290609730000125
Wherein, the sigmaaIs the standard deviation sigma of the position error of the anchor nodea(m)；
Corresponding to the measurement vector
Figure GDA0003290609730000131
Is Gaussian distributed and the covariance matrix of the measurement vectors is Rr,nAll measurement vectors
Figure GDA0003290609730000132
Is Gaussian distributed and has a covariance matrix of all measurement vectors of Rr＝Bdiag(Rr,1,Rr,2,…,Rr,N)。
In the above scheme, the configuration portion 803 is configured to:
generating an estimate
Figure GDA0003290609730000133
Wherein,
Figure GDA0003290609730000134
qoand toRepresenting the rotation angle estimate and the translation vector estimate, A, of the target solid body, respectivelyoA position information estimator representing the anchor node, the rotation angle Q of the target solid body having the following relationship to the target solid body rotation matrix Q:
q＝[α,β,γ]T，Q＝QγQβQα，
Figure GDA0003290609730000135
Figure GDA0003290609730000136
wherein the target solid rotation matrix Q is a special orthogonal group
Figure GDA0003290609730000137
According to the estimated quantity psioDetermining a log-likelihood function of the estimator as:
Figure GDA0003290609730000138
wherein, eta is a constant value,
Figure GDA0003290609730000139
in the above scheme, the estimating part 804 is configured to:
log-likelihood function lnp (ζ, ψ) according to maximum likelihood rule based on the estimatoro) The corresponding least squares model is generated as follows:
Figure GDA00032906097300001310
wherein R ═ Bdiag (R)r,RA)；
Set initial value ζo(ψ)＝ζo(ψ{0})+G{0}(ψ-ψ{0}) Wherein G is{0}Is a gradient matrix and
Figure GDA00032906097300001311
wherein,
Figure GDA0003290609730000141
Figure GDA0003290609730000142
wherein，
Figure GDA0003290609730000143
Figure GDA0003290609730000144
Figure GDA0003290609730000145
Is QoRelative to qoA derivative of (a);
based on the initial value, iteration is carried out according to the set iteration times and an iteration equation shown in the following formula for the least square model according to a Gauss-Newton iteration method, and the maximum likelihood solution of the estimator is obtained
Figure GDA0003290609730000146
ψ{k+1}＝ψ{k}+(G{k}TR-1G{k})-1G{k}TR-1(ζ-ζo(ψ{k})),k＝0,1,...
Where k represents the number of iterations.
Referring to fig. 9, the apparatus 80 further comprises: a CRLB generating section 805 and a comparing section 806; wherein the CRLB generating section 805 is configured to determine a cramer-circle lower bound of the rotation angle estimate and the translation vector estimate of the target solid body from the estimate and a log-likelihood function of the estimate;
the comparing portion 806 is configured to obtain a mean square error of a maximum likelihood solution of the estimator at a plurality of anchor node position error strengths and compare the mean square error to a cramer-circle lower bound for each anchor node position error strength.
In the above scheme, the CRLB generating section 805 is configured to:
log likelihood function lnp (ζ, ψ) of the estimatoro) For the estimation
Figure GDA0003290609730000147
Calculating the second partial derivative to obtain the Fisher snow information momentMatrix of
Figure GDA0003290609730000148
Wherein,
Figure GDA0003290609730000149
Figure GDA00032906097300001410
Figure GDA00032906097300001411
wherein,
Figure GDA0003290609730000151
Figure GDA0003290609730000152
Figure GDA0003290609730000153
is QoRelative to qoA derivative of (a);
FIM (psi) based on the snow information matrixo) Respectively obtaining the estimators psi by a block matrix inversion formulaoIn
Figure GDA0003290609730000154
Lower boundary of Cramer-O
Figure GDA0003290609730000155
And A ofoCrLB (A) of the lower CrLB boundaryo)＝FIM(Ao)-1＝(Z-YTX-1Y)-1。
It is understood that in this embodiment, "part" may be part of a circuit, part of a processor, part of a program or software, etc., and may also be a unit, and may also be a module or a non-modular.
In addition, each component in the embodiment may be integrated in one processing unit, or each unit may exist alone physically, or two or more units are integrated in one unit. The integrated unit can be realized in a form of hardware or a form of a software functional module.
Based on the understanding that the technical solution of the present embodiment essentially or a part contributing to the prior art, or all or part of the technical solution may be embodied in the form of a software product stored in a storage medium, and include several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) or a processor (processor) to execute all or part of the steps of the method of the present embodiment. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
Therefore, the present embodiment provides a computer storage medium, which stores a solid body positioning program, and when the solid body positioning program is executed by at least one processor, the method of positioning a solid body in the above technical solution is implemented.
Based on the solid body positioning apparatus 80 and the computer storage medium, referring to fig. 10, a specific hardware structure of the solid body positioning apparatus 80 provided by the embodiment of the invention is shown, including: a communication interface 1001, a memory 1002, and a processor 1003; the various components are coupled together by a bus system 1004. It is understood that the bus system 1004 is used to enable communications among the components. The bus system 1004 includes a power bus, a control bus, and a status signal bus in addition to a data bus. But for the sake of clarity the various busses are labeled in fig. 10 as the bus system 1004. Wherein,
the communication interface 1001 is used for receiving and sending signals in the process of receiving and sending information with other external network elements;
the memory 1002 is used for storing a computer program capable of running on the processor 1003;
the processor 1003 is configured to, when running the computer program, perform the following steps:
determining direction information and position information of a target solid body based on sensors distributed on the target solid body; and the number of the first and second groups,
acquiring noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein the anchor nodes comprise anchor nodes having a position error; and the number of the first and second groups,
constructing an estimator according to the direction information and the position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and acquiring a log-likelihood function of the estimator; wherein the estimates comprise a direction estimate and a location estimate of the target solid body, and a location estimate of the anchor node; and the number of the first and second groups,
based on the log-likelihood function of the estimator, acquiring a maximum likelihood solution of the estimator according to a set maximum likelihood estimation algorithm strategy; wherein the maximum likelihood solution for the estimator comprises a direction estimate and a position estimate for the target solid state volume, and a position estimate for the anchor node.
It is to be understood that the memory 1002 in embodiments of the present invention may be either volatile memory or nonvolatile memory, or may include both volatile and nonvolatile memory. The non-volatile Memory may be a Read-Only Memory (ROM), a Programmable ROM (PROM), an Erasable PROM (EPROM), an Electrically Erasable PROM (EEPROM), or a flash Memory. Volatile Memory can be Random Access Memory (RAM), which acts as external cache Memory. By way of illustration and not limitation, many forms of RAM are available, such as Static random access memory (Static RAM, SRAM), Dynamic Random Access Memory (DRAM), Synchronous Dynamic random access memory (Synchronous DRAM, SDRAM), Double Data Rate Synchronous Dynamic random access memory (ddr Data Rate SDRAM, ddr SDRAM), Enhanced Synchronous SDRAM (ESDRAM), Synchlink DRAM (SLDRAM), and Direct Rambus RAM (DRRAM). The memory 1002 of the systems and methods described herein is intended to comprise, without being limited to, these and any other suitable types of memory.
And the processor 1003 may be an integrated circuit chip having signal processing capabilities. In implementation, the steps of the above method may be implemented by integrated logic circuits of hardware or instructions in the form of software in the processor 1003. The Processor 1003 may be a general-purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), an off-the-shelf Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic device, or discrete hardware components. The various methods, steps and logic blocks disclosed in the embodiments of the present invention may be implemented or performed. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. The steps of the method disclosed in connection with the embodiments of the present invention may be directly implemented by a hardware decoding processor, or implemented by a combination of hardware and software modules in the decoding processor. The software module may be located in ram, flash memory, rom, prom, or eprom, registers, etc. storage media as is well known in the art. The storage medium is located in the memory 1002, and the processor 1003 reads the information in the memory 1002 and performs the steps of the above method in combination with the hardware thereof.
It is to be understood that the embodiments described herein may be implemented in hardware, software, firmware, middleware, microcode, or any combination thereof. For a hardware implementation, the Processing units may be implemented within one or more Application Specific Integrated Circuits (ASICs), Digital Signal Processors (DSPs), Digital Signal Processing Devices (DSPDs), Programmable Logic Devices (PLDs), Field Programmable Gate Arrays (FPGAs), general purpose processors, controllers, micro-controllers, microprocessors, other electronic units configured to perform the functions described herein, or a combination thereof.
For a software implementation, the techniques described herein may be implemented with modules (e.g., procedures, functions, and so on) that perform the functions described herein. The software codes may be stored in a memory and executed by a processor. The memory may be implemented within the processor or external to the processor.
Specifically, when the processor 1003 is further configured to run the computer program, the method steps for positioning the solid body in the foregoing technical solution are executed, which is not described herein again.
It should be noted that: the technical schemes described in the embodiments of the present invention can be combined arbitrarily without conflict.
The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the appended claims.

Claims (9)

1. A method of solid body positioning, the method comprising:
determining direction information and position information of a target solid body based on sensors distributed on the target solid body;
acquiring noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein the anchor nodes comprise anchor nodes having a position error;
constructing an estimator according to the direction information and the position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and acquiring a log-likelihood function of the estimator; wherein the estimates comprise a direction estimate and a location estimate of the target solid body, and a location estimate of the anchor node;
based on the log-likelihood function of the estimator, acquiring a maximum likelihood solution of the estimator according to a set maximum likelihood estimation algorithm strategy; wherein the maximum likelihood solution for the estimator comprises a direction estimate and a position estimate for the target solid state volume, and a position estimate for the anchor node;
wherein the obtaining a maximum likelihood solution of the estimator according to a set maximum likelihood estimation algorithm strategy based on the log-likelihood function of the estimator comprises:
according to the maximum likelihood rule, a corresponding least squares model is generated from the log-likelihood function ln p (ζ, ψ) of the estimator as follows:
Figure FDA0003290609720000011
wherein R ═ Bdiag (R)r,RA)，
Figure FDA0003290609720000012
Wherein R isrA covariance matrix representing all measurement vectors; rAA noise covariance matrix representing the anchor node; r represents said all measurement vectors; a represents the anchor node location matrix;
set initial value ζo(ψ)＝ζo(ψ{0})+G{0}(ψ-ψ{0}) Wherein G is{0}Is a gradient matrix and
Figure FDA0003290609720000013
wherein,
Figure FDA0003290609720000014
Figure FDA0003290609720000021
wherein,
Figure FDA0003290609720000022
Figure FDA0003290609720000023
Figure FDA0003290609720000024
is QoRelative to qoA derivative of (a);
wherein M represents the number of anchor nodes; n represents the number of said sensors;
Figure FDA0003290609720000025
representing the position information of the Nth sensor under a global reference frame; m represents the anchor node identification, M is 1,2, …, M; n represents the sensor identification, N is 1,2, …, N; q. q.soRepresenting an estimated rotation angle of the target solid body;
based on the initial value, iteration is carried out according to the set iteration times and an iteration equation shown in the following formula for the least square model according to a Gauss-Newton iteration method, and the maximum likelihood solution of the estimator is obtained
Figure FDA0003290609720000026
ψ{k+1}＝ψ{k}+(G{k}TR-1G{k})-1G{k}TR-1(ζ-ζo(ψ{k})),k＝0,1,...
Where k represents the number of iterations.
2. The method of claim 1, wherein determining the directional information and the positional information of the target solid body based on sensors distributed on the target solid body comprises:
in response to the transmissionThe number of the sensors is N, the relative positions of the sensors are known, and the position information of the sensors under a local reference system is
Figure FDA0003290609720000027
Wherein N represents a sensor identifier, N is 1,2, …, N;
determining the position information of each sensor under the global reference system based on the position information of each sensor under the local reference system
Figure FDA0003290609720000028
Wherein,
Figure FDA0003290609720000029
q and t are respectively a rotation matrix and a translation vector from a local reference frame to a global reference frame and are used for representing the direction information and the position information of the target solid body.
3. The method of claim 1, wherein the obtaining noise covariance information for anchor nodes distributed around the target solid body and measurement vectors comprises:
corresponding to the number of the anchor nodes being M, the position information of the anchor nodes under the global reference frame is amWherein M represents an anchor node identifier, M is 1,2, …, M;
generating a position matrix of the anchor node according to the position information of the anchor node
Figure FDA00032906097200000210
A noise covariance matrix of the anchor node corresponding to a zero-mean and independently identically distributed Gaussian random process of the position error of the anchor node
Figure FDA0003290609720000031
Wherein, the sigmaaIs the standard deviation sigma of the position error of the anchor nodea(m)；
Corresponding to the measurement vector
Figure FDA0003290609720000032
Is Gaussian distributed and the covariance matrix of the measurement vectors is Rr,nAll measurement vectors
Figure FDA0003290609720000033
Is Gaussian distributed and has a covariance matrix of all measurement vectors of Rr＝Bdiag(Rr,1,Rr,2,…,Rr,N)。
4. The method of any one of claims 1 to 3, wherein constructing an estimator from the directional and positional information of the target solid body and the noise covariance information of the anchor node and the measurement vector and obtaining a log-likelihood function of the estimator comprises:
generating an estimate
Figure FDA0003290609720000034
Wherein,
Figure FDA0003290609720000035
qoand toRepresenting the rotation angle estimate and the translation vector estimate, A, of the target solid body, respectivelyoA position information estimator representing the anchor node, the rotation angle Q of the target solid body having the following relationship to the target solid body rotation matrix Q:
q＝[α,β,γ]T，Q＝QγQβQα，
Figure FDA0003290609720000036
Figure FDA0003290609720000037
wherein the target solid rotation matrix Q is a special orthogonal group
Figure FDA0003290609720000038
Determining a log-likelihood function of the estimate based on the estimate ψ as:
Figure FDA0003290609720000039
wherein, eta is a constant value,
Figure FDA00032906097200000310
5. the method of claim 1, further comprising:
determining a cramer-circle lower bound of a rotation angle estimator and a translation vector estimator of the target solid body from the estimator and a log-likelihood function of the estimator;
the mean square error of the maximum likelihood solution of the estimator is obtained under a plurality of anchor node position error strengths and compared with the lower clalmelo bound of each anchor node position error strength.
6. The method of claim 5, wherein determining the Cramer Role lower bound for the rotation angle estimate and the translation vector estimate of the target solid body from the estimate and a log likelihood function of the estimate comprises:
applying a log-likelihood function lnp (ζ, ψ) of the estimate to the estimate
Figure FDA0003290609720000041
Calculating the second partial derivative to obtain a Fisher snow information matrix
Figure FDA0003290609720000042
Wherein,
Figure FDA0003290609720000043
Figure FDA0003290609720000044
Figure FDA0003290609720000045
wherein,
Figure FDA0003290609720000046
Figure FDA0003290609720000047
Figure FDA0003290609720000048
is QoRelative to qoA derivative of (a);
respectively obtaining the estimators psi by a block matrix inversion formula based on the Fisher-Tropsch information matrix FIM (psi)
Figure FDA0003290609720000049
Lower boundary of Cramer-O
Figure FDA00032906097200000410
And A ofoCrLB (A) of the lower CrLB boundaryo)＝FIM(Ao)-1＝(Z-YTX-1Y)-1。
7. An apparatus for solid body positioning, the apparatus comprising: a determination section, an acquisition section, a construction section and an estimation section; wherein,
the determination section is configured to determine direction information and position information of a target solid body based on sensors distributed on the target solid body;
the acquisition part is configured to acquire noise covariance information of anchor nodes distributed around the target solid body and a measurement vector; wherein the anchor nodes comprise anchor nodes having a position error;
the constructing part is configured to construct an estimator according to the direction information and the position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and acquire a log-likelihood function of the estimator; wherein the estimates comprise a direction estimate and a location estimate of the target solid body, and a location estimate of the anchor node;
the estimation part is configured to obtain a maximum likelihood solution of the estimator according to a set maximum likelihood estimation algorithm strategy based on a log-likelihood function of the estimator; wherein the maximum likelihood solution for the estimator comprises a direction estimate and a position estimate for the target solid state volume, and a position estimate for the anchor node;
wherein the estimation section is configured to:
from the log-likelihood function lnp (ζ, ψ) of the estimator, a corresponding least squares model is generated as follows according to the maximum likelihood rule:
Figure FDA0003290609720000051
wherein R ═ Bdiag (R)r,RA)，
Figure FDA0003290609720000052
Wherein R isrA covariance matrix representing all measurement vectors; rAA noise covariance matrix representing the anchor node; r represents said all measurement vectors; a represents the anchor node location matrix;
set initial value ζo(ψ)＝ζo(ψ{0})+G{0}(ψ-ψ{0}) Wherein G is{0}Is a gradient matrix and
Figure FDA0003290609720000053
wherein,
Figure FDA0003290609720000054
Figure FDA0003290609720000055
wherein,
Figure FDA0003290609720000056
Figure FDA0003290609720000057
Figure FDA0003290609720000058
is QoRelative to qoA derivative of (a);
wherein M represents the number of anchor nodes; n represents the number of said sensors;
Figure FDA0003290609720000059
representing the position information of the Nth sensor under a global reference frame; m represents the anchor node identification, M is 1,2, …, M; n represents the sensor identification, N is 1,2, …, N; q. q.soRepresenting an estimated rotation angle of the target solid body;
based on the initial value, iteration is carried out according to the set iteration times and an iteration equation shown in the following formula for the least square model according to a Gauss-Newton iteration method, and the maximum likelihood solution of the estimator is obtained
Figure FDA0003290609720000061
ψ{k+1}＝ψ{k}+(G{k}TR-1G{k})-1G{k}TR-1(ζ-ζo(ψ{k})),k＝0,1,...
Where k represents the number of iterations.
8. An apparatus for solid body positioning, the apparatus comprising: a communication interface, a memory and a processor; the communication interface is used for receiving and sending signals in the process of receiving and sending information with other external network elements;
the memory for storing a computer program operable on the processor;
the processor, when executing the computer program, is configured to perform the method steps of solid body positioning according to any of claims 1 to 6.
9. A computer storage medium, characterized in that the computer storage medium stores a program for solid body positioning, which when executed by at least one processor implements the method steps of solid body positioning according to any one of claims 1 to 6.
CN201910134391.9A 2019-02-22 2019-02-22 Solid body positioning method and device and computer storage medium Active CN109959918B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910134391.9A CN109959918B (en) 2019-02-22 2019-02-22 Solid body positioning method and device and computer storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910134391.9A CN109959918B (en) 2019-02-22 2019-02-22 Solid body positioning method and device and computer storage medium

Publications (2)

Publication Number Publication Date
CN109959918A CN109959918A (en) 2019-07-02
CN109959918B true CN109959918B (en) 2021-11-09

Family

ID=67023845

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910134391.9A Active CN109959918B (en) 2019-02-22 2019-02-22 Solid body positioning method and device and computer storage medium

Country Status (1)

Country Link
CN (1) CN109959918B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113411881B (en) * 2021-05-28 2022-06-14 海南大学 RSS unmanned aerial vehicle cluster distributed positioning method

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104965189B (en) * 2014-07-22 2018-05-29 浙江银江研究院有限公司 Indoor occupant localization method based on maximal possibility estimation
CN105891773A (en) * 2014-12-15 2016-08-24 江南大学 Improved source localization algorithm under sensor error condition
CN106992822A (en) * 2017-03-29 2017-07-28 国网重庆市电力公司电力科学研究院 A kind of localization method of the blind node of wireless sensor network
CN107040992B (en) * 2017-06-07 2019-08-27 江西理工大学 Wireless sensor network node locating method and device
CN108200547B (en) * 2017-11-30 2020-07-14 宁波大学 Rigid body positioning method based on measured distance
CN108535687B (en) * 2018-03-20 2021-05-18 西安电子科技大学 Indoor wireless positioning method based on TOF and RSSI information fusion

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
RSS Distribution-Based Passive Localization and Its Application in Sensor Networks;Liu, Chen et al.;《IEEE TRANSACTIONS ON WIRELESS COMMUNICATIONS》;20161231;第15卷(第4期);2883-2895 *

Also Published As

Publication number Publication date
CN109959918A (en) 2019-07-02

Similar Documents

Publication Publication Date Title
Ryu et al. Navigation system heading and position accuracy improvement through GPS and INS data fusion
CN110865398B (en) Processing method and processing device for positioning data, terminal equipment and storage medium
CN111208492A (en) Vehicle-mounted laser radar external parameter calibration method and device, computer equipment and storage medium
Ali et al. Kalman filter tracking
CN112834995B (en) Vehicle millimeter wave radar angle calibration method and system and electronic equipment
CN114019954B (en) Course installation angle calibration method, device, computer equipment and storage medium
CN109769206B (en) Indoor positioning fusion method and device, storage medium and terminal equipment
Masmitja et al. Range-only single-beacon tracking of underwater targets from an autonomous vehicle: From theory to practice
JP5605539B2 (en) MOBILE POSITION ESTIMATION TRACKING DEVICE, MOBILE POSITION ESTIMATION TRACKING METHOD, AND MOBILE POSITION ESTIMATION TRACKING PROGRAM
CN109959918B (en) Solid body positioning method and device and computer storage medium
CN114501615B (en) Terminal positioning method, terminal positioning device, computer equipment, storage medium and program product
CN112396662A (en) Method and device for correcting conversion matrix
WO2017090359A1 (en) Attitude angle calculation device and attitude angle calculation method
CN109375159B (en) Pure orientation weighting constraint total least square positioning method
CN109343013B (en) Spatial registration method and system based on restarting mechanism
CN114894222B (en) External parameter calibration method of IMU-GNSS antenna and related method and equipment
Cheng et al. RANSAC-based instantaneous real-time kinematic positioning with GNSS triple-frequency signals in urban areas
CN115790603A (en) Unmanned aerial vehicle dynamic target estimation method used in information rejection environment
JP5180447B2 (en) Carrier phase relative positioning apparatus and method
US10976447B2 (en) Azimuth angle calculating device, and method of calculating azimuth angle
CN112782647B (en) Information-combined quadratic constraint least square radiation source positioning method
CN111123323B (en) Method for improving positioning precision of portable equipment
KR101509569B1 (en) Signal Source Location Determine Method using Earth Radius and Signal Directional Information
CN106546961A (en) A kind of variable step constrains total least square spatial registration algorithm
Trigo et al. Vehicle heading estimation using a two low-cost GPS receiver configuration

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