Method and apparatus for state estimation, and electronic device and storage medium

Abstract

Provided in the present disclosure are a method and apparatus for state estimation, and an electronic device and a storage medium. A specific implementation comprises: acquiring state variables, first data which is collected by a first inertial measurement unit arranged in a first device, and second data which is collected by a second inertial measurement unit arranged in a second device, wherein the state variables comprise: a target parameter, relative positions and relative poses of the second device and the first device, an angular velocity bias and an acceleration bias of the first inertial measurement unit, and an angular velocity bias and an acceleration bias of the second inertial measurement unit, and the target parameter comprises at least one of the following two pieces of information: the relative velocity between the second device and the first device, and a projection result obtained by projecting the relative velocity between the second device and the first device in an inertial coordinate system to a reference coordinate system; acquiring a state estimation equation meeting a predetermined lever arm constraint condition; and on the basis of the state variables, the first data, the second data and the state estimation equation, determining estimated state variables.

Description

Method, device, electronic device and storage medium for state estimation

[0001] This disclosure claims priority to the Chinese patent application filed with the China Patent Office on December 29, 2023, with application number CN202311865612.2, and invention name “Method, device, electronic device and storage medium for state estimation”, the entire contents of which are incorporated by reference into this disclosure. Technical Field

[0002] The present disclosure relates to the field of relative positioning technology, and in particular to a method, device, electronic device, and storage medium for state estimation. Background Art

[0003] In some application scenarios, it is necessary to perform relative positioning of two devices without paying attention to the absolute positioning of the two devices. Summary of the Invention

[0004] According to one aspect of the present disclosure, a method for state estimation is provided, comprising: obtaining a state quantity, first data collected by a first inertial measurement unit provided on a first device, and second data collected by a second inertial measurement unit provided on a second device; wherein the state quantity comprises: a target parameter, a relative position and a relative attitude between the second device and the first device, an angular velocity bias and an acceleration bias of the first inertial measurement unit, and an angular velocity bias and an acceleration bias of the second inertial measurement unit, and the target parameter comprises at least one of the following two: a relative velocity between the second device and the first device, and a projection result obtained by projecting the relative velocity between the second device and the first device in an inertial coordinate system onto a reference coordinate system, where the reference coordinate system is a coordinate system corresponding to one of the first inertial measurement unit and the second inertial measurement unit; obtaining a state estimation equation that satisfies a predetermined lever arm constraint condition; wherein the predetermined lever arm constraint condition is defined by at least one of the following first equation and second equation, the first equation defining the relative velocity between the second device and the first device, and the second equation defining the relative position between the second device and the first device; and determining an estimated state quantity based on the state quantity, the first data, the second data, and the state estimation equation.

[0005] According to another aspect of the present disclosure, there is provided an apparatus for state estimation, comprising: a first acquisition module, for acquiring a state quantity, first data collected by a first inertial measurement unit provided on a first device, and second data collected by a second inertial measurement unit provided on a second device; wherein the state quantity comprises: a target parameter, a relative position and a relative attitude between the second device and the first device, an angular velocity bias and an acceleration bias of the first inertial measurement unit, and an angular velocity bias and an acceleration bias of the second inertial measurement unit, and the target parameter comprises at least one of the following two: a relative velocity between the second device and the first device, a first velocity in an inertial coordinate system, and a second velocity in an inertial coordinate system. A projection result is obtained by projecting the relative speed between the second device and the first device onto a reference coordinate system, where the reference coordinate system is a coordinate system corresponding to one of the first inertial measurement unit and the second inertial measurement unit; a second acquisition module is used to obtain a state estimation equation that meets a predetermined lever arm constraint condition; wherein the predetermined lever arm constraint condition is defined by at least one of the following first equation and second equation, the first equation defines the relative speed between the second device and the first device, and the second equation defines the relative position between the second device and the first device; a first determination module is used to determine an estimated state quantity based on the state quantity, the first data, the second data, and the state estimation equation.

[0006] According to yet another aspect of the present disclosure, a computer-readable storage medium is provided, wherein the storage medium stores a computer program for executing the above-mentioned method for state estimation.

[0007] According to another aspect of the present disclosure, an electronic device is provided, comprising: a processor; a memory for storing processor-executable instructions; and the processor, configured to read the executable instructions from the memory and execute the instructions to implement the above-mentioned method for state estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0008] FIG. 1 is a schematic diagram of a lever arm between a first inertial measurement unit and a second inertial measurement unit in some exemplary embodiments of the present disclosure.

[0009] FIG2 is a flowchart of a method for state estimation provided by some exemplary embodiments of the present disclosure.

[0010] FIG3 is a flowchart of a method for correcting a priori state quantities provided by some exemplary embodiments of the present disclosure.

[0011] FIG4-1 is a schematic diagram of application scenarios of some exemplary embodiments of the present disclosure.

[0012] FIG4-2 is a schematic diagram of application scenarios of some other exemplary embodiments of the present disclosure.

[0013] FIG4-3 is a schematic diagram of application scenarios of some further exemplary embodiments of the present disclosure.

[0014] FIG5 is a flowchart of a method for correcting a priori state quantities provided by other exemplary embodiments of the present disclosure.

[0015] FIG6 is a flowchart of a method for determining a discrete-time error state transfer matrix provided by some exemplary embodiments of the present disclosure.

[0016] FIG7 is a flowchart of a method for correcting a priori state quantities provided by some further exemplary embodiments of the present disclosure.

[0017] FIG8-1 is a flowchart of a method for correcting a priori state quantities provided by some further exemplary embodiments of the present disclosure.

[0018] FIG8-2 is a flowchart of a method for correcting a priori state quantities provided by some further exemplary embodiments of the present disclosure.

[0019] FIG9 is a flowchart of a method for correcting an estimated state quantity provided by some exemplary embodiments of the present disclosure.

[0020] FIG10 is a flowchart of a method for determining an estimated state quantity provided by some exemplary embodiments of the present disclosure.

[0021] FIG11 is a schematic structural diagram of an apparatus for state estimation provided by some exemplary embodiments of the present disclosure.

[0022] FIG12 is a schematic diagram of modules involved in the correction of a priori state quantities in some exemplary embodiments of the present disclosure.

[0023] FIG13 is a schematic diagram of modules involved in the correction of a priori state quantities in some other exemplary embodiments of the present disclosure.

[0024] FIG14-1 is a schematic diagram of modules involved in the correction of a priori state quantities in some further exemplary embodiments of the present disclosure.

[0025] FIG14-2 is a schematic diagram of modules involved in the correction of a priori state quantities in some further exemplary embodiments of the present disclosure.

[0026] FIG15 is a schematic diagram of modules involved in the correction of a priori state quantities in still other exemplary embodiments of the present disclosure.

[0027] FIG16 is a schematic diagram of modules involved in the correction of a priori state quantities in yet other exemplary embodiments of the present disclosure.

[0028] FIG17 is a schematic structural diagram of any one of the second determination module, the fifth determination module, the first determination submodule, and the fourth determination submodule in some exemplary embodiments of the present disclosure.

[0029] FIG18 is a schematic diagram of modules involved in correction of estimated state quantities in still other exemplary embodiments of the present disclosure.

[0030] FIG19 is a schematic structural diagram of a first determination module in some exemplary embodiments of the present disclosure.

[0031] FIG20 is a structural diagram of an electronic device provided by some exemplary embodiments of the present disclosure. DETAILED DESCRIPTION

[0032] To explain the present disclosure, example embodiments of the present disclosure will be described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present disclosure, rather than all the embodiments. It should be understood that the present disclosure is not limited to the example embodiments.

[0033] It should be noted that the relative arrangement of components and steps, the numerical expressions and numerical values ​​set forth in these embodiments do not limit the scope of the present disclosure unless specifically stated otherwise.

[0034] Exemplary Overview

[0035] In some application scenarios, it is necessary to perform relative positioning of two devices.

[0036] For example, one of the two devices may be a head-mounted display device, and the other may be a movable platform on which the head-mounted display device is located. Also, for example, one of the two devices may be a head-mounted display device, and the other may be an adapter device that is communicatively connected to the head-mounted display device. A head-mounted display device may also be referred to as a head-mounted display (HMD) or a head display. A head-mounted display device may be presented in the form of glasses, a helmet, etc. A head-mounted display device may include but is not limited to augmented reality (AR) glasses, virtual reality (VR) glasses, etc. The movable platform may include but is not limited to a vehicle, a ship, an airplane, a train, etc. By relative positioning the head-mounted display device and the movable platform, or the adapter device, the relative posture between the head-mounted display device and the movable platform, or the adapter device may be obtained. With reference to the relative position between the head-mounted display device and the movable platform, or the adapter device, the virtual image displayed by the head-mounted display device can be rendered, adjusted, distorted, controlled, etc. to achieve a specific display effect, such as a 6dof (degree of freedom) display effect.

[0037] For another example, one of the two devices could be a drone, and the other could be the mobile platform onto which the drone will land. By relative positioning the drone and the mobile platform, the relative pose of the drone and the mobile platform can be determined. Using this relative pose, the motion parameters of the drone and the mobile platform can be controlled to ensure a smooth landing of the drone onto the mobile platform.

[0038] For another example, one of the two devices could be a first aircraft in flight that needs to refuel, and the other could be a second aircraft in flight that is being supplied with fuel. By relative positioning the first and second aircraft, the relative position of the first and second aircraft can be determined. Based on this relative position, the flight parameters of the first and second aircraft can be controlled to ensure smooth refueling of the first aircraft by the second aircraft.

[0039] Therefore, how to effectively achieve relative positioning between two devices is a problem worthy of attention for those skilled in the art.

[0040] Exemplary Methods

[0041] In order to effectively achieve relative positioning between two devices, an embodiment of the present disclosure provides a method for state estimation. For ease of distinction, hereinafter, one of the two devices requiring state estimation may be referred to as the first device, and the other as the second device.

[0042] In some optional embodiments of the present disclosure, the first device may be a movable platform on which a head-mounted display device is located, and the second device may be the head-mounted display device.

[0043] In other optional embodiments of the present disclosure, the first device may be a portable device communicatively connected to a head-mounted display device, and the second device may be the head-mounted display device. The portable device and the head-mounted display device may be used in immobile environments, or the portable device may be removably secured to a movable platform on which the head-mounted display device resides, and the portable device and the head-mounted display device may be used in conjunction. Removable securing methods may include, but are not limited to, screwing, snapping, and plugging.

[0044] When used in an immobile scenario, the method provided in the present disclosure can be used to estimate the state between the portable device and the head-mounted display device.

[0045] When used in a mobile platform scenario, since the portable device and the mobile platform are relatively fixed, as long as the state estimation between the head-mounted display device and the portable device is achieved, the state estimation between the head-mounted display device and the mobile platform can be achieved on this basis.

[0046] Of course, the types of the first device and the second device can also be interchangeable. For example, the first device can be a head-mounted display device, and the second device can be a mobile platform.

[0047] The first device and the second device may be respectively provided with an inertial measurement unit (IMU). For the sake of distinction, the inertial measurement unit provided on the first device may be referred to as a first inertial measurement unit, and the inertial measurement unit provided on the second device may be referred to as a second inertial measurement unit. It is understood that an inertial measurement unit is a device that uses an accelerometer and a gyroscope to measure the acceleration and angular velocity of an object. In this way, the first inertial measurement unit can be used to measure the angular velocity and acceleration of the first device in the world coordinate system, and the second inertial measurement unit can be used to measure the angular velocity and acceleration of the second device in the world coordinate system.

[0048] It should be noted that the system including the first device and the second device can be considered as a dual IMU system. The dual IMU system involves a first inertial measurement unit provided on the first device and a second inertial measurement unit provided on the second device. Since the first inertial measurement unit and the second inertial measurement unit cannot overlap in space, there is a lever arm between the first inertial measurement unit and the second inertial measurement unit. For details, please refer to Figure 1. This will bring about a lever arm effect and generate a lever arm error, thereby affecting the accuracy of state estimation. For example, if the distance between the first inertial measurement unit and the second inertial measurement unit is 1 meter, if the lever arm error is not processed, the lever arm error can be 1 meter.

[0049] In view of this, in an embodiment of the present disclosure, a state estimation equation that takes into account the lever arm error is provided. The state estimation equation may refer to: an equation for determining the state of relative motion between the second device and the first device based on observation and measurement. Lever arm related constraints may be introduced into the state estimation equation. State estimation may be performed by combining the data collected by the first inertial measurement unit and the second inertial measurement unit, and the state estimation equation. The estimated state may be used to achieve relative positioning between the second device and the first device. In this way, the lever arm error can be eliminated as much as possible, the accuracy of the state estimation can be improved, and thus the relative positioning accuracy between the second device and the first device can be improved.

[0050] As shown in FIG2 , which is a flowchart of a method for state estimation provided by some exemplary embodiments of the present disclosure, the method shown in FIG2 may include steps 210 , 220 , and 230 .

[0051] Step 210, obtaining state quantities, first data collected by a first inertial measurement unit provided on the first device, and second data collected by a second inertial measurement unit provided on the second device; wherein the state quantities include: target parameters, the relative position and relative posture between the second device and the first device, the angular velocity bias and acceleration bias of the first inertial measurement unit, and the angular velocity bias and acceleration bias of the second inertial measurement unit, and the target parameters include at least one of the following two: the relative velocity between the second device and the first device, and the projection result obtained by projecting the relative velocity between the second device and the first device in the inertial coordinate system onto the reference coordinate system, where the reference coordinate system is the coordinate system corresponding to one of the first inertial measurement unit and the second inertial measurement unit.

[0052] In some optional embodiments of the present disclosure, the first inertial measurement unit and the second inertial measurement unit may both have corresponding coordinate systems. The coordinate system corresponding to the first inertial measurement unit may be referred to as the first coordinate system. The coordinate system corresponding to the second inertial measurement unit may be referred to as the second coordinate system. The first coordinate system may refer to a three-dimensional coordinate system constructed with the center of mass or other position point of the first inertial measurement unit as the origin. The second coordinate system may refer to a three-dimensional coordinate system constructed with the center of mass or other position point of the second inertial measurement unit as the origin. In an optional example, the first coordinate system may be the three-dimensional coordinate system with the origin being 0 in FIG. m , the three coordinate axes are X m 、Y m 、Z m The second coordinate system can be the three-dimensional coordinate system with the origin O in Figure 1 b , the three coordinate axes are X b 、Y b 、Z b 3D coordinate system.

[0053] It should be noted that the reference coordinate system can be a first coordinate system or a second coordinate system. If the reference coordinate system is the first coordinate system, the relative position between the second device and the first device can be understood as: the position of the second device in the first coordinate system. If the reference coordinate system is the second coordinate system, the relative position between the second device and the first device can be understood as: the position of the first device in the second coordinate system. The relative speed and relative posture between the second device and the first device can be understood in a similar way and will not be repeated here. In addition, if the reference coordinate system is the first coordinate system, the relative speed between the second device and the first device in the inertial coordinate system can be understood as: the speed difference obtained by subtracting the speed of the second device in the inertial coordinate system from the speed of the first device in the inertial coordinate system. If the reference coordinate system is the second coordinate system, the relative speed between the second device and the first device in the inertial coordinate system can be understood as: the speed difference obtained by subtracting the speed of the first device in the inertial coordinate system from the speed of the second device in the inertial coordinate system.

[0054] In some optional embodiments of the present disclosure, a state estimator can be used to perform state estimation at certain time intervals. When executing the method of the embodiment of the present disclosure, various state estimators can be used. For example, the state estimator can be a Kalman filter or other types of state estimators. If the state estimator used in the embodiment of the present disclosure is a Kalman filter, the state quantity obtained in step 210 may include: the current latest state quantity. If the state estimator used in the embodiment of the present disclosure is other types of state estimators, the state quantity obtained in step 210 may include: the current latest state quantity, or include: the current latest state quantity and several historical state quantities.

[0055] In some optional embodiments of the present disclosure, the first data acquired in step 210 may include: the most recent angular velocity data and acceleration data acquired by the first inertial measurement unit before the current state estimation, and historical angular velocity data and historical acceleration data acquired by the first inertial measurement unit before the current state estimation. The type of the second data acquired in step 210 can be found in the description of the first data and is not further described here.

[0056] In some optional embodiments of the present disclosure, the reference coordinate system may be the first coordinate system, the relative velocity between the second device and the first device may be expressed as v, and the projection result obtained by projecting the relative velocity between the second device and the first device in the inertial coordinate system onto the reference coordinate system may be expressed as v'. v and v' may satisfy the following formula: v' = v + ω1 × p

[0057] Wherein, ω1 represents the angular velocity of the first inertial measurement unit, and p represents the relative position between the second device and the first device.

[0058] Step 220: Obtain a state estimation equation that satisfies a predetermined lever arm constraint condition; wherein the predetermined lever arm constraint condition is defined by at least one of the following first equation and second equation, wherein the first equation defines the relative speed between the second device and the first device, and the second equation defines the relative position between the second device and the first device.

[0059] As described above, there is a lever arm between the first inertial measurement unit and the second inertial measurement unit, which will bring about a lever arm effect and generate a lever arm error. The size of the lever arm effect can be proportional to the target cross product result. The target cross product result is the result of cross product of the following two: the angular velocity of the inertial measurement unit corresponding to the reference coordinate system, and the relative position between the second device and the first device. The lever arm effect can only be ignored when the angular velocity of the inertial measurement unit corresponding to the reference coordinate system is 0, or the first inertial measurement unit and the second inertial measurement unit coincide with each other. Obviously, the size of the lever arm effect is closely related to the relative position between the second device and the first device. It can be understood that by deriving the relative position between the second device and the first device, the relative velocity between the second device and the first device can be obtained. Then, it can be considered that the size of the lever arm effect is also related to the relative velocity between the second device and the first device.

[0060] In view of this, in some embodiments, a first equation defining the relative velocity between the second device and the first device can be introduced into the state estimation equation. In other embodiments, a second equation defining the relative position between the second device and the first device can be introduced. In this way, it can be considered that lever-related constraints are introduced into the state estimation equation, and the state estimation equation complies with the predetermined lever constraint conditions.

[0061] Step 230 : determining an estimated state quantity based on the state quantity, the first data, the second data, and the state estimation equation.

[0062] Taking the case where the state quantities obtained in step 210 only include the most recent state quantities as an example, the estimated state quantities determined in step 230 may include: estimated parameters corresponding to each parameter in the state quantities obtained in step 210. For example, if the target parameter is the relative velocity between the second device and the first device, the estimated state quantities determined in step 230 may include: an estimated relative position corresponding to the relative position in the state quantities obtained in step 210, an estimated relative attitude corresponding to the relative attitude in the state quantities obtained in step 210, an estimated relative velocity corresponding to the relative velocity in the state quantities obtained in step 210, an estimated angular velocity bias corresponding to the angular velocity bias of the first inertial measurement unit in the state quantities obtained in step 210, an estimated acceleration bias corresponding to the acceleration bias of the first inertial measurement unit in the state quantities obtained in step 210, an estimated angular velocity bias corresponding to the angular velocity bias of the second inertial measurement unit in the state quantities obtained in step 210, and an estimated acceleration bias corresponding to the acceleration bias of the second inertial measurement unit in the state quantities obtained in step 210.

[0063] In some optional embodiments of the present disclosure, the estimated relative position and the estimated relative posture in the estimated state quantity may be directly used as the relative positioning result between the second device and the first device, and the relative positioning result may be output.

[0064] In some other optional embodiments of the present disclosure, the position and attitude can be further optimized based on the estimated relative position and the estimated relative attitude to obtain a relative positioning result between the second device and the first device, and the relative positioning result can be output.

[0065] In an embodiment of the present disclosure, state estimation can be performed by combining the state quantity, the data collected by the first and second inertial measurement units, and the state estimation equation that meets the predetermined lever arm constraint conditions to obtain an estimated state quantity. In this way, the lever arm between the first and second inertial measurement units is taken into account during the state estimation process, which helps to eliminate the lever arm error and obtain the true relative speed and relative attitude between the second device and the first device. Therefore, the embodiment of the present disclosure can improve the accuracy of state estimation, thereby improving the relative positioning accuracy between the second device and the first device.

[0066] Optionally, the estimated state quantity determined in the embodiments of the present disclosure can be used as both a priori state quantity and an observation used in the update (correction) process. For example, the result of the estimated state quantity is used as observation data to update the priori state quantity. It is understandable that in some algorithms, the priori state quantity can also be referred to as a predicted state quantity, an initial value, etc.

[0067] Alternatively, the state quantity of the state estimation equation can be expressed as follows:

[0068] Optionally, the state quantity can be maintained all the time. When the estimated state quantity determined in the embodiment of the present disclosure is used as the prior state quantity, the above state quantity can be estimated and the above state quantity can be used to perform subsequent required operations. When the estimated state quantity determined in the embodiment of the present disclosure is used as an observation, the required state quantity can be selected as the observation data. When the estimated state quantity determined in the embodiment of the present disclosure is used as an observation, the present disclosure does not limit the calculation method of the prior state quantity, and the prior value or initial value of the state quantity defined in the present disclosure can be calculated.

[0069] In some optional embodiments of the present disclosure, a first equation defines a relationship between a derivative of the relative velocity between the first and second devices and a target difference, where the target difference is the difference between the reference relative acceleration and the Coriolis acceleration and the drag acceleration. The reference relative acceleration is associated with the relative attitude between the first and second devices, the first corrected acceleration, and the second corrected acceleration. The first corrected acceleration is the difference between the acceleration measured by the first inertial measurement unit and the acceleration bias of the first inertial measurement unit. The second corrected acceleration is the difference between the acceleration measured by the second inertial measurement unit and the acceleration bias of the second inertial measurement unit. The Coriolis acceleration is associated with the corrected angular velocity and the relative velocity between the first and second devices. The corrected angular velocity is the difference between the angular velocity measured by the inertial measurement unit corresponding to the reference coordinate system and the angular velocity bias of the inertial measurement unit. The drag acceleration is associated with the corrected angular velocity, the relative position between the first and second devices, and the derivative of the corrected angular velocity.

[0070] In an optional example, if the inertial measurement unit corresponding to the reference coordinate system is a first inertial measurement unit, that is, the reference coordinate system is the first coordinate system, the first equation may include:

[0071] in, Indicates the relative speed between the second device and the first device. The symbol · above any parameter indicates a derivative operation. C() indicates converting the parameters in () from quaternion form to rotation matrix form. Indicates the relative posture between the second device and the first device, represents the second corrected acceleration, represents the first corrected acceleration, represents the reference relative acceleration, represents the Coriolis acceleration, represents the corrected angular velocity, and Can form the involved acceleration, Indicates the relative position of the second device to the first device.

[0072] It should be noted that the Coriolis acceleration is the acceleration that occurs when the first coordinate system and the second coordinate system have relative motion. The Coriolis acceleration involves the magnitude of the lever arm effect. and The induced acceleration is the acceleration caused by the motion of the reference coordinate system. The induced acceleration involves the magnitude of the lever arm effect. and Therefore, through the first equation, the lever arm related constraints can be effectively introduced.

[0073] In some optional embodiments of the present disclosure, if the inertial measurement unit corresponding to the reference coordinate system is a first inertial measurement unit, that is, the reference coordinate system is the first coordinate system, the second equation may include:

[0074] in, represents the projection result of the relative velocity between the second device and the first device in the inertial coordinate system onto the reference coordinate system. It should be noted that the meanings of the other parameters involved in the second equation can be referred to the relevant explanations of the meanings of the parameters involved in the first equation above, and will not be repeated here.

[0075] It should be noted that and The following formula can be satisfied:

[0076] Recombination As well as the specific formula involved in the first equation above, the specific formula of the second equation above can be derived. and They are all related to the size of the lever arm effect. Through the second equation, the constraints related to the lever arm can also be effectively introduced.

[0077] Optionally, if the state estimation equation includes the first equation mentioned above, in addition to the first equation, the state estimation equation may further include the following equation:

[0078] Optionally, if the state estimation equation includes the second equation above, in addition to the second equation, the state estimation equation may further include the following equation:

[0079] in, represents the angular velocity bias of the first inertial measurement unit, represents the acceleration bias of the first inertial measurement unit, represents the angular velocity bias of the second inertial measurement unit, Indicates the acceleration bias of the second inertial measurement unit.

[0080] Hereinafter, the estimated state quantity determined in the embodiment of the present disclosure may be used as a priori state quantity as an example for explanation. It is understandable that the relevant description does not mean that the estimated state quantity determined in the present disclosure can be used as an observation.

[0081] FIG3 is a flow chart illustrating a method for correcting a priori state variables according to some exemplary embodiments of the present disclosure. The method illustrated in FIG3 may include steps 310, 320, 330, 340, 350, and 360. Optionally, the method illustrated in FIG3 may be performed after step 230 of the present disclosure.

[0082] Step 310: Obtain a state covariance matrix of an error state quantity corresponding to the state quantity, a first type of analytical solution determined based on preset definition information, and observation data obtained by observing the first device and the second device through a target sensor; wherein the preset definition information refers to: definition information of parameters in a discrete-time error state transfer matrix, the first type of analytical solution refers to: an analytical solution of parameters in a discrete-time error state transfer matrix, and the type of the target sensor is different from that of an inertial measurement unit.

[0083] Optionally, the target sensor may be, for example, an image sensor as shown in FIG. 4-1 and FIG. 4-2 .

[0084] Step 310 can also be: obtaining the state covariance matrix of the error state quantity corresponding to the state quantity, the first type of analytical solution determined based on preset definition information, and the observation data; wherein the preset definition information refers to: the definition information of the parameters in the discrete-time error state transfer matrix, and the first type of analytical solution refers to: the analytical solution of the parameters in the discrete-time error state transfer matrix.

[0085] Taking the case where the target parameter is the relative speed between the second device and the first device as an example, the error state quantity corresponding to the state quantity can be expressed as follows:

[0086] in, The error parameters corresponding to the relative position, relative velocity, relative attitude, angular velocity bias of the first inertial measurement unit, angular velocity bias of the second inertial measurement unit, acceleration bias of the first inertial measurement unit, and acceleration bias of the second inertial measurement unit in the state quantities obtained in step 210 may be represented in sequence. The state covariance matrix obtained in step 310 may include the covariance between the error parameters in the error state quantities.

[0087] In some optional embodiments of the present disclosure, an analytical method can be used based on preset definition information to derive an analytical solution for the parameters in the discrete-time error state transfer matrix to obtain a first-class analytical solution. In addition, the first-class analytical solution can also be stored in a preset first storage area. In step 310, the first-class analytical solution can be obtained from the first storage area. It can be understood that the analytical solution is a concept corresponding to the numerical solution. The analytical solution can be considered as a strict formula. Given any independent variable, the independent variable is substituted into the formula for calculation to obtain the corresponding response variable.

[0088] Step 320 : Determine a discrete-time error state transfer matrix based on the state quantity, the priori state quantity, the first data, the second data, and the first type of analytical solution.

[0089] Step 330: Obtain the noise Jacobian matrix and the continuous-time noise covariance matrix.

[0090] In some optional implementations of the present disclosure, the noise Jacobian matrix can be obtained by real-time calculation based on the state quantity, the priori state quantity, the first data, and the second data.

[0091] In some optional embodiments of the present disclosure, the continuous-time noise covariance matrix may be pre-set or may need to be calculated in real time. In an optional example, the continuous-time noise covariance matrix may be determined using the following formula:

[0092]

[0093] Among them, Q c (t) represents the continuous-time noise covariance matrix at time t, E[] represents the expectation of the contents in calculation [], express The amount of state noise, n T (t) represents the transpose of the state noise amount at time t.

[0094] Step 340 : Determine a discrete-time noise covariance matrix based on the discrete-time error state transfer matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix.

[0095] In some optional embodiments of the present disclosure, the discrete-time noise covariance matrix is ​​expressed as Q k,k-1 , the discrete-time noise covariance matrix is ​​calculated as follows:

[0096] in, express The discrete-time error state transfer matrix between time instant k and time instant k-1, express The noise Jacobian matrix between time instant and time instant k-1, Q c represents the continuous-time noise covariance matrix.

[0097] In this way, by bringing the discrete-time error state transfer matrix, the noise Jacobian matrix and the continuous-time noise covariance matrix into the above formula for calculation, the discrete-time error state transfer matrix can be obtained efficiently and reliably.

[0098] Step 350 : Determine a priori state covariance matrix based on the discrete-time error state transfer matrix, the discrete-time noise covariance matrix, and the state covariance matrix.

[0099] In some optional embodiments of the present disclosure, a priori state covariance matrix can be obtained by real-time calculation based on the discrete-time error state transfer matrix, the discrete-time noise covariance matrix, and the state covariance matrix. In an optional example, the discrete-time error state transfer matrix is ​​represented as Φ, the discrete-time noise covariance matrix is ​​represented as Q, and the state covariance matrix is ​​represented as P. The priori state covariance matrix can be calculated based on Φ, P, the transpose of Φ, Q, the state noise amount mentioned above, and the transpose of Q.

[0100] Step 360: Modify the prior state quantity based on the prior state covariance matrix and the observation data.

[0101] In some optional embodiments of the present disclosure, the a priori state covariance matrix can be used to determine the Kalman gain, and the observation data and the Kalman gain can be used to determine the a posteriori error. By combining the a priori state quantity with the a posteriori error, the correction of the a priori state quantity can be achieved. The correction result of the a priori state quantity can be used as the corrected state quantity. The corrected state quantity may include: correction parameters corresponding to each a priori parameter in the a priori state quantity. That is, the corrected state quantity may include: a corrected relative position corresponding to the a priori relative position in the a priori state quantity, and a corrected relative attitude corresponding to the a priori relative attitude in the a priori state quantity. The corrected relative position and the corrected relative attitude can be used as the relative positioning result between the second device and the first device. The relative positioning result can then be output. In addition, the corrected state quantity can be used as the latest state quantity.

[0102] In the embodiment shown in FIG3 , the first type of analytical solution determined based on the preset definition information can be used to determine the discrete-time error state transfer matrix. This helps ensure the reliability of the determined discrete-time error state transfer matrix. Next, the discrete-time noise covariance matrix and the prior state covariance matrix can be determined in sequence, and the reliability of the prior state covariance matrix can also be well guaranteed. In this way, using the prior state covariance matrix and observation data to correct the prior state quantity can well ensure the correction effect.

[0103] For example, referring to the method shown in FIG3 , an Error State Kalman Filter (ESKF) may be used to perform state estimation.

[0104] FIG5 is a flow chart illustrating a method for correcting a priori state variables according to another exemplary embodiment of the present disclosure. The method shown in FIG5 may include steps 510, 520, 530, 540, 550, and 560. Optionally, the method shown in FIG5 may be performed after step 230 of the present disclosure.

[0105] Step 510: Obtain a first type of analytical solution determined based on preset definition information, and observation data obtained by observing the first device and the second device through the target sensor; wherein the preset definition information refers to: definition information of parameters in the discrete-time error state transfer matrix, the first type of analytical solution refers to: analytical solution of parameters in the discrete-time error state transfer matrix, and the type of the target sensor is different from the inertial measurement unit.

[0106] Step 510 can also be to obtain a first-type analytical solution and observation data determined based on preset definition information; wherein the preset definition information refers to: definition information of parameters in the discrete-time error state transfer matrix, and the first-type analytical solution refers to: analytical solution of parameters in the discrete-time error state transfer matrix.

[0107] Step 520 : Determine a discrete-time error state transfer matrix based on the state quantity, the priori state quantity, the first data, the second data, and the first type of analytical solution.

[0108] Step 530: Obtain the noise Jacobian matrix and the continuous-time noise covariance matrix.

[0109] Step 540 : Determine a discrete-time noise covariance matrix based on the discrete-time error state transfer matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix.

[0110] For example, the specific implementation of steps 510 to 540 can also refer to the above introduction to steps 310 to 340, which will not be repeated here.

[0111] Step 550: Determine a target Jacobian matrix of the error state quantity corresponding to the observed data, the first data, and the second data relative to the state quantity.

[0112] For example, the specific composition of the error state quantity corresponding to the state quantity can also refer to the relevant introduction of step 310, which will not be repeated here.

[0113] In some optional embodiments of the present disclosure, a first Jacobian matrix of the error state quantity corresponding to the observed data relative to the state quantity can be determined, a second Jacobian matrix of the error state quantity corresponding to the first data relative to the state quantity can be determined, and a third Jacobian matrix of the error state quantity corresponding to the first data relative to the state quantity can be determined. The target Jacobian matrix can include the first Jacobian matrix, the second Jacobian matrix, and the third Jacobian matrix. Of course, in a specific implementation, the observed data, the first data, and the second data can also be regarded as a whole, and the fourth Jacobian matrix of the error state quantity corresponding to the state quantity can be calculated for this whole. The fourth Jacobian matrix can be used as the target Jacobian matrix.

[0114] Step 560: Modify the prior state quantity based on the discrete-time noise covariance matrix, the target Jacobian matrix, the observation data, the first data, and the second data.

[0115] In some optional embodiments of the present disclosure, a posterior error can be determined based on the discrete-time noise covariance matrix, the target Jacobian matrix, the observed data, the first data, and the second data. The a priori state quantity can be combined with the posterior error to achieve correction of the a priori state quantity. The correction result of the a priori state quantity can be used as the corrected state quantity.

[0116] Optionally, multiple correction operations may be performed in step 560. For example, the prior state quantity may be corrected using the observed data to obtain a first correction result, and then the first correction result may be corrected using the first data and the second data to obtain a second correction result. The second correction result may serve as the final corrected state quantity.

[0117] In the embodiment shown in Figure 5, the first type of analytical solution determined based on the preset definition information can be used to determine the discrete-time error state transfer matrix, which is conducive to ensuring the reliability of the determined discrete-time error state transfer matrix. Next, the discrete-time noise covariance matrix and the target Jacobian matrix can be determined in sequence, and the reliability of the discrete-time noise covariance matrix and the target Jacobian matrix can also be well guaranteed. The discrete-time noise covariance matrix, the target Jacobian matrix, the observation data, the first data, and the second data can all be used to correct the prior state quantity. In this way, the data used for correcting the prior state quantity is very rich, for example, including both observation data and data collected by the inertial measurement unit, which can better guarantee the correction effect.

[0118] For example, referring to the method shown in FIG5 , other state estimators besides the Kalman filter may be used for state estimation.

[0119] 6 , a method for determining a discrete-time error state transfer matrix is ​​introduced, which can be used in the methods shown in FIG. 3 and FIG. 5 .

[0120] FIG6 is a flow chart illustrating a method for determining a discrete-time error state transfer matrix according to some exemplary embodiments of the present disclosure. The method shown in FIG6 may include steps 610 and 620. Alternatively, the combination of steps 610 and 620 may serve as an alternative implementation of step 320 of FIG3. The combination of steps 610 and 620 may also serve as an alternative implementation of step 520 of FIG5.

[0121] Step 610: Obtain a processing result obtained by processing the first type of analytical solution according to a preset method; wherein the preset method includes at least one of the following: Runge-Kutta method, numerical approximation method.

[0122] In some optional embodiments of the present disclosure, both the Runge-Kutta method and numerical approximation methods can be used to process angular velocity, acceleration, velocity, translation, and the like. The Runge-Kutta method may include, but is not limited to, the third-order Runge-Kutta method, the fourth-order Runge-Kutta method, the fifth-order Runge-Kutta method, and the like. It is understood that the Runge-Kutta method is a high-precision, single-step algorithm widely used in engineering. Given known derivatives and initial value information for an equation, computer simulation technology can be used to eliminate the complex process of solving differential equations. Numerical approximation methods may include, but are not limited to, the zero-order approximation method, the first-order approximation method, the second-order approximation method, and the like.

[0123] The zero-order approximation method can be understood as follows: assuming that a parameter needs to be integrated for a period of time, the starting value of the parameter during this period can be used as the zero-order approximation result, or the final value of the parameter during this period can be used as the zero-order approximation result, or the average of the starting value and final value of the parameter during this period can be used as the zero-order approximation result, or the average or weighted average of the starting value, intermediate value, and final value of the parameter during this period can be used as the zero-order approximation result.

[0124] The first-order approximation method can be understood as follows: during the integration process, the angle can be processed by uniform angular acceleration, the acceleration can be processed by uniform acceleration, the velocity can be processed by uniform acceleration, and the translation can be processed by uniform speed.

[0125] The second-order approximation method can be understood as follows: assuming that a parameter needs to be integrated over a period of time, the integration is performed according to the formula A = a + bt + (1 / 2)ct², where a, b, and c represent coefficients and t represents time. If the parameter is angular velocity, the start time of this period is t0, a can be the angular velocity at t0, b can be the angular acceleration at t0, and c can be the angular jerk at t0. Similarly, if the parameter is velocity, a can be the velocity at t0, b can be the acceleration at t0, and c can be the jerk at t0.

[0126] In some optional embodiments of the present disclosure, the first type of analytical solution can be processed according to a preset method to obtain a processing result. For example, if the first type of analytical solution contains analytical solutions corresponding to multiple parameters, all analytical solutions in the first type can be divided into two categories: analytical solutions with high complexity and analytical solutions with low complexity. If some analytical solutions only involve simple logical operations such as single integrals or addition and subtraction, these analytical solutions can be classified as analytical solutions with low complexity. If some analytical solutions involve multiple integrals or other complex logical operations, these analytical solutions can be classified as analytical solutions with high complexity. For analytical solutions with high complexity, at least one of the Runge-Kutta method and a numerical approximation method can be introduced to simplify the analytical solution to obtain a simplified result of the analytical solution. For analytical solutions with low complexity, neither the Runge-Kutta method nor the numerical approximation method is required. In this way, the processing result can include: a simplified result of the analytical solution with high complexity and an analytical solution with low complexity, and the processing result can be considered a simplified result of the analytical solution of the first type. Optionally, the processing result can be stored in a preset second storage area. In step 610 , the processing result may be obtained from the second storage area.

[0127] Step 620 : Determine a discrete-time error state transfer matrix based on the state quantity, the priori state quantity, the first data, the second data, and the processing result.

[0128] Since the processing result can be a simplified result of the first type of analytical solution, in step 620, the state quantity, prior state quantity, first data, and second data can all be brought into the simplified result of the first type of analytical solution for calculation, so that the discrete-time error state transfer matrix can be obtained efficiently and reliably.

[0129] It should be noted that, because the first-type analytical solution is a rigorous formula derived through derivation, it can very accurately reflect the operational logic required to obtain the discrete-time error state transfer matrix. By introducing at least one of the Runge-Kutta method and a numerical approximation method to process the first-type analytical solution, it can be simplified to a certain extent. Thus, using the processed results of the first-type analytical solution to determine the discrete-time error state transfer matrix can not only ensure the reliability of the calculated discrete-time error state transfer matrix, but also reduce computational complexity, increase computational speed, and improve the efficiency of obtaining the discrete-time error state transfer matrix.

[0130] Of course, the implementation method for determining the discrete-time error state transfer matrix is ​​not limited to this. For example, if the overall complexity of the first type of analytical solution is not too high, the state quantity, the prior state quantity, the first data, and the second data can be directly brought into the first type of analytical solution for calculation to obtain the discrete-time error state transfer matrix.

[0131] Some further exemplary embodiments of the present disclosure provide a method for estimating state quantities, which may include: obtaining a first-type analytical solution determined based on preset definition information; wherein the preset definition information refers to: definition information of parameters in a discrete-time error state transfer matrix, and the first-type analytical solution refers to: an analytical solution of parameters in a discrete-time error state transfer matrix.

[0132] For specific implementation methods, please refer to the explanations of other embodiments of the present disclosure and will not be repeated here.

[0133] The method may further include taking the estimated state quantity as the priori state quantity, and correcting the priori state quantity based on the state quantity, the priori state quantity, the first data, the second data, and the first type of analytical solution.

[0134] The method may further include obtaining a priori state quantities, and correcting the priori state quantities based on the priori state quantities, estimated state quantities, first data, second data, and a first type of analytical solution, wherein at least part of the estimated state quantities is used as observation data.

[0135] As shown in Figure 7, it is a flowchart of a method for correcting a priori state quantity provided by some exemplary embodiments of the present disclosure. The method shown in Figure 7 may include steps 710 and 720. Optionally, the method shown in Figure 7 may be performed after step 230 of the present disclosure.

[0136] Step 710, obtaining a first type of analytical solution determined based on preset definition information; wherein the preset definition information refers to: definition information of parameters in the discrete-time error state transfer matrix, and the first type of analytical solution refers to: analytical solution of parameters in the discrete-time error state transfer matrix.

[0137] For example, referring to the relevant introduction in the embodiment shown in FIG3 , an analytical method can be used based on preset definition information to derive analytical solutions of the parameters in the discrete-time error state transfer matrix to obtain a first type of analytical solution.

[0138] Step 720: Modify the priori state quantity based on the state quantity, the priori state quantity, the first data, the second data, and the first type of analytical solution.

[0139] In some optional implementations of the present disclosure, as shown in FIG8-1 , step 720 may include:

[0140] Step 7201: Obtain the state covariance matrix and observation data of the error state quantity corresponding to the state quantity.

[0141] Step 7203: Determine a discrete-time error state transfer matrix based on the state quantity, the priori state quantity, the first data, the second data, and the first type of analytical solution.

[0142] Step 7205: Obtain the noise Jacobian matrix and the continuous-time noise covariance matrix.

[0143] Step 7207 : Determine the discrete-time noise covariance matrix based on the discrete-time error state transfer matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix.

[0144] Step 7209: Determine the priori state covariance matrix based on the discrete-time error state transfer matrix, the discrete-time noise covariance matrix, and the state covariance matrix.

[0145] Step 7211, based on the prior state covariance matrix and observation data, the prior state quantity is corrected.

[0146] For example, the specific implementation process and achievable effects of the implementation shown in FIG8-1 may refer to the relevant introduction to the embodiment shown in FIG3 above, and will not be repeated here.

[0147] In some other optional embodiments of the present disclosure, as shown in FIG8-2 , step 720 may include:

[0148] Step 7213, obtain observation data.

[0149] Step 7215: Determine a discrete-time error state transfer matrix based on the state quantity, the priori state quantity, the first data, the second data, and the first type of analytical solution.

[0150] Step 7217, obtain the noise Jacobian matrix and the continuous-time noise covariance matrix.

[0151] Step 7219: Determine the discrete-time noise covariance matrix based on the discrete-time error state transfer matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix.

[0152] Step 7221, determine the target Jacobian matrix of the error state quantity corresponding to the observed data, the first data and the second data relative to the state quantity.

[0153] Step 7223, based on the discrete-time noise covariance matrix, the target Jacobian matrix, the observation data, the first data and the second data, the prior state quantity is corrected.

[0154] For example, the specific implementation process and achievable effects of the implementation shown in FIG8-2 may be referred to the above description of the embodiment shown in FIG5 , and will not be repeated here.

[0155] In the embodiment shown in FIG7 , since the first type of analytical solution is a rigorous formula obtained through derivation, using the first type of analytical solution for the correction of the priori state quantity is conducive to ensuring the correction effect.

[0156] As shown in Figure 9, it is a flowchart of a method for correcting an estimated state quantity provided by some exemplary embodiments of the present disclosure. The method shown in Figure 9 may include steps 910, 920, 930, 940, and 950. Optionally, the method shown in Figure 9 may be performed after step 230 of the present disclosure. The embodiment shown in Figure 9 may be applicable to various state estimation methods, for example, it may be used in the embodiment related to Figure 3 of the present disclosure, it may also be used in the embodiment related to Figure 5 of the present disclosure, or in other state estimators.

[0157] Step 910: Obtain the continuous-time error state transfer matrix and observation data.

[0158] The continuous-time error state transfer matrix can be expressed as F, and the continuous-time error state transfer matrix is ​​calculated as follows:

[0159] Among them, 03 represents the 0 matrix, I3 represents the identity matrix, [×] represents the antisymmetric matrix of the parameters in the calculation []. represents the difference between the angular velocity measured by the first inertial measurement unit and the angular velocity bias of the first inertial measurement unit, represents the difference between the acceleration measured by the second inertial measurement unit and the acceleration bias of the second inertial measurement unit, Represents the difference between the angular velocity measured by the second inertial measurement unit and the angular velocity bias of the second inertial measurement unit.

[0160] Step 920: Determine a discrete-time error state transfer matrix based on the continuous-time error state transfer matrix.

[0161] In some optional embodiments of the present disclosure, a discrete-time error state transfer matrix can be obtained by Taylor expansion based on the continuous-time error state transfer matrix. Assuming that the continuous-time error state transfer matrix is ​​represented by F and the discrete-time error state transfer matrix is ​​represented by Φ, the discrete-time error state transfer matrix can be determined as follows:

[0162] Among them, the specific first few terms of the Taylor expansion may depend on the requirements for accuracy.

[0163] Of course, the method for determining the discrete-time error state transfer matrix is ​​not limited to this. For example, the continuous-time error state transfer matrix and the discrete-time error state transfer matrix can be associated. Assume that the discrete-time error state transfer matrix is ​​represented by Φ(t, t0). F and Φ(t, t0) can generally satisfy the following formula:

[0164] Then, based on the formula in the previous paragraph, we can use analytical methods to derive analytical solutions for the parameters in the discrete-time error state transfer matrix to obtain a first-kind analytical solution. The first-kind analytical solution can then be used to determine the discrete-time error state transfer matrix.

[0165] Step 930: Obtain the noise Jacobian matrix and the continuous-time noise covariance matrix.

[0166] Step 940 : Determine a discrete-time noise covariance matrix based on the discrete-time error state transfer matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix.

[0167] For example, the specific implementation of step 930 and step 940 can refer to the above introduction to step 330 and step 340, which will not be repeated here.

[0168] Step 950: Correct the estimated state quantity based on the discrete-time noise covariance matrix and the observation data.

[0169] In some optional embodiments of the present disclosure, the estimated state quantity can be used as a priori state quantity. Please refer to the introduction in the embodiment shown in Figure 3. First, based on the discrete-time error state transfer matrix, the discrete-time noise covariance matrix and the state covariance matrix, the priori state covariance matrix is ​​determined, and then based on the priori state covariance matrix and the observation data, the priori state quantity is corrected.

[0170] In other optional embodiments of the present disclosure, the estimated state quantity can be used as a priori state quantity. Referring to the introduction in the embodiment shown in Figure 5, the target Jacobian matrix of the error state quantity corresponding to the observed data, the first data, and the second data relative to the state quantity can be determined, and the priori state quantity can be corrected based on the discrete-time noise covariance matrix, the target Jacobian matrix, the observed data, the first data, and the second data.

[0171] In the embodiment shown in FIG9 , when the continuous-time error state transfer matrix is ​​known, the discrete-time error state transfer matrix can be determined based on the continuous-time error state transfer matrix. Next, the discrete-time noise covariance matrix can be determined, and the discrete-time noise covariance matrix and the observed data can be used to correct the estimated state quantity. This allows correction of the estimated state quantity without having to predetermine a first-class analytical solution.

[0172] In some optional embodiments of the present disclosure, the observation data may include at least one of the following:

[0173] a preset observed relative position between the second device and the first device;

[0174] Data obtained by observing the first device and the second device through a target sensor, where the target sensor is of a type different from an inertial measurement unit;

[0175] First data collected by the first inertial measurement unit and second data collected by the second inertial measurement unit.

[0176] It should be noted that the case where the observed data includes at least one of the above can be applied to any scenario where the prior state quantity needs to be corrected. For example, it can be applied to the method shown in Figure 8-1, the method shown in Figure 8-2, or the method shown in Figure 9.

[0177] Optionally, the preset observation relative position may be a fixed value, which may be a value set in advance according to an application scenario.

[0178] For example, when the first device is a movable device that is in communication with the second device and the user controls the second device using the movable device, the preset observation relative position can be a relatively small value between 0 and 2 meters, which is consistent with the possible distance between the two devices in this case.

[0179] For example, if the first device is a movable device that is communicatively connected to the second device, the movable device is removably connected to a movable platform, and the relative position and relative posture between the movable device and the movable platform are fixed, the preset observed relative position can be a relatively large value between 1 and 8 meters. This is consistent with the possible distance between the two devices in this case.

[0180] In some optional embodiments of the present disclosure, the target sensor may include a visual sensor, such as an image sensor, or a non-visual sensor. For example, the target sensor may include: a first magnetometer provided on a first device and a second magnetometer provided on a second device, and accordingly, the observation data may include: first magnetic measurement data collected by the first magnetometer and second magnetic measurement data collected by the second magnetometer. For another example, the target sensor may include one of the following two: a first image sensor provided on the first device and a second image sensor provided on the second device, and accordingly, the observation data may include one of the following two: an image of the second device collected by the first image and an image of the first device collected by the second device.

[0181] In an optional example, as shown in FIG4-1 , the first device may be a portable device 415 removably fixed to a vehicle 410, and the second device may be augmented reality glasses 420 worn by rear passengers in the vehicle 410. An image sensor may be provided on the surface of the portable device 415, and the observation data may include images of the augmented reality glasses 420 captured by the image sensor provided on the portable device 415.

[0182] In another optional example, as shown in FIG4-2 , the first device may be a vehicle 410, and the second device may be augmented reality glasses 420 worn by rear passengers in the vehicle. The augmented reality glasses 420 may be provided with an image sensor, and the observation data may include images captured by the image sensor provided on the augmented reality glasses 420.

[0183] In another optional example, as shown in FIG4-3 , the first device may be augmented reality glasses 430 worn by a user, and the second device may be an adapter device 440 communicatively connected to the augmented reality glasses 430. The augmented reality glasses 430 and the adapter device 440 may each be provided with a magnetometer, and the observation data may include: magnetic measurement data collected by the magnetometer provided on the augmented reality glasses 430 and the magnetometer provided on the adapter device 440, respectively.

[0184] In the embodiments of the present disclosure, at least one of a preset observed relative position, data obtained by the target sensor through observation, first data collected by the first inertial measurement unit, and second data collected by the second inertial measurement unit can be used to correct the a priori state quantity. This provides a very effective reference for correcting the a priori state quantity, which helps ensure the correction effect.

[0185] In some optional embodiments of the present disclosure, the parameter located in the i-th row and j-th column of the discrete-time error state transfer matrix can be expressed as Φ ij (t, t0), then the first type of analytical solution may include at least one of the following: Φ 12 (t,t0)=C′(t)(t-t0)

[0186] If i = 3, and j = 1, 2, 6, 7, Φ 3j (t,t0)=03

[0187] If i>3, and i=j, Φ ij (t,t0)=I3

[0188] If i>3 and i≠j, Φ ij (t,t0)=03

[0189] in,

[0190] in, represents the attitude of the second inertial measurement unit at time t0 relative to the second inertial measurement unit at time t, t0 represents the initial time of error state transfer, t represents the target time of error state transfer, represents the attitude of the first inertial measurement unit at time s relative to the second inertial measurement unit at time t0, where s is between t0 and t, represents the attitude of the second inertial measurement unit at time s relative to the second inertial measurement unit at time t0, represents the attitude of the first inertial measurement unit at time t0 relative to the first inertial measurement unit at time t, I represents the identity matrix, [×] represents the antisymmetric matrix for calculating the parameters in [], represents the difference between the angular velocity at time t0 measured by the first inertial measurement unit and the angular velocity bias of the first inertial measurement unit at time t0, represents the difference between the angular velocity at time t measured by the first inertial measurement unit and the angular velocity bias of the first inertial measurement unit at time t, express The attitude of the first inertial measurement unit at time t relative to the first inertial measurement unit at time t0, express The second inertial measurement unit at time The attitude of the first inertial measurement unit at the moment, Indicates the second inertial measurement unit measured The acceleration of the moment and The difference between the acceleration bias of the second inertial measurement unit at time t, The second inertial measurement unit at time t0 is relative to The attitude of the first inertial measurement unit at the moment, Between t0 and s, The first inertial measurement unit at time μ is relative to The attitude of the first inertial measurement unit at time t0, μ is located between t0 and between, The second inertial measurement unit at time μ is relative to The attitude of the first inertial measurement unit at the moment, express The attitude of the second inertial measurement unit at time t0 relative to the first inertial measurement unit at time t0, 03 represents the 0 matrix, and I3 represents the identity matrix.

[0191] The following is an introduction to the principle of obtaining the first type of analytical solution.

[0192] As introduced above, for example, the continuous-time error state transfer matrix can be expressed as F, which can be in the following form:

[0193] The discrete-time error state transfer matrix can be expressed as Φ(t, t0). F and Φ(t, t0) can satisfy the following formula:

[0194] The formula in the above paragraph can be used to reflect the definition information of each parameter in the discrete-time error state transfer matrix, that is, the formula in the above paragraph can carry preset definition information.

[0195] The parameter in row i and column j of Φ(t,t0) can be expressed as Φ ij (t,t0) or Φ ij Next, let’s discuss the case where i>3. In this case, we can have the following formula: Φ ij (t,t0)=Φ ij (t0,t0)=I3 or 03

[0196] Still assuming i>3, if i=j, then: Φ ij (t,t0)=Φ ij (t0,t0)=I3

[0197] Still assuming i>3, if i≠j, then: Φ ij (t,t0)=Φ ij (t0,t0)=03

[0198] Then, Φ(t,t0) as a whole can be in the following matrix form:

[0199] Next we discuss the case where i≤3.

[0200] Alternatively, Φ can be determined according to the following derivation process: 31 , Φ 32 , Φ 36 , Φ 37 : Φ 3j (t0,t0)=03 Φ 3j (t, t0) = 03, j = 1, 2, 6, 7

[0201] Alternatively, Φ can be determined according to the following derivation process: 33 :

[0202] Alternatively, Φ can be determined according to the following derivation process: 34 :

[0203] Alternatively, Φ can be determined according to the following derivation process: 35 :

[0204] Alternatively, Φ can be determined according to the following derivation process: 11 and Φ 21 : Φ 11 (t0,t0)=I3 Φ 21 (t0,t0)=03

[0205] Alternatively, Φ can be determined according to the following derivation process: 12 and Φ 22 : Φ 12 (t0)=03 Φ 22 (t0)=I3 Φ 12 =C′(t)(t-t0)

[0206] Alternatively, Φ can be determined according to the following derivation process: 13 and Φ 23 : Φ 13 (t0,t0)=03 Φ 23 (t0,t0)=03

[0207] Alternatively, Φ can be determined according to the following derivation process: 14 and Φ 24 : Φ 14 (t0,t0)=03 Φ 24 (t0,t0)=03

[0208] Alternatively, Φ can be determined according to the following derivation process: 15 and Φ 25 : Φ 15 (t0,t0)=03 Φ 25 (t0,t0)=03

[0209] Alternatively, Φ can be determined according to the following derivation process: 16 and Φ 26 : Φ 16 (t0,t0)=03 Φ 26 (t0,t0)=03

[0210] Alternatively, Φ can be determined according to the following derivation process: 17 and Φ 27 : Φ17 (t0,t0)=03 Φ 27 (t0,t0)=03

[0211] At this point, based on the required relationship between F and Φ(t, t0), a rigorous mathematical derivation has yielded analytical solutions for each parameter in the discrete-time error state transfer matrix, resulting in a first-class analytical solution. Using this first-class analytical solution for determining the discrete-time error state transfer matrix helps ensure the reliability of the resulting discrete-time error state transfer matrix.

[0212] It should be noted that the specific expression for F described above is only one possible form, and those skilled in the art may also adopt other expressions based on actual circumstances. If other expressions are adopted, the corresponding analytical solution can also be obtained through rigorous mathematical derivation in combination with the required relationship between F and Φ(t, t0).

[0213] As shown in Figure 10, it is a flowchart of a method for determining an estimated state quantity provided by some exemplary embodiments of the present disclosure. The method shown in Figure 10 may include steps 1010 and 1020. Optionally, a combination of steps 1010 and 1020 may be used as an optional implementation of step 230 of the present disclosure.

[0214] Step 1010: Obtain a second type of analytical solution determined based on the state estimation equation and a preset method; wherein the preset method includes at least one of the following: a Runge-Kutta method, a numerical approximation method, and the second type of analytical solution refers to an analytical solution of a parameter in the estimated state quantity.

[0215] Step 1020: Determine an estimated state quantity based on the state quantity, the first data, the second data, and the second type of analytical solution.

[0216] It should be noted that the various equations included in the state estimation equation introduced above involve multiple operational logics, and there is a coupling relationship between many parameters involved in these equations. The integration results of some parameters in the state estimation equation may be very complicated, making it difficult to derive the analytical solutions of various parameters in the estimated state quantity from the state estimation equation.

[0217] In view of this, in some optional embodiments of the present disclosure, before executing the method shown in FIG10 , at least one of the Runge-Kutta method and a numerical approximation method may be introduced to simplify the state estimation equation to obtain a second-class analytical solution. The specific use of the Runge-Kutta method and the numerical approximation method is described in the embodiment shown in FIG6 and will not be repeated here. The second-class analytical solution can then be stored in a preset third storage area.

[0218] In step 1010, the second type of analytical solution can be obtained from the third storage area. In step 1020, the state quantity, the first data, and the second data are all brought into the second type of analytical solution for calculation to efficiently and reliably obtain the corresponding estimated state quantity.

[0219] Using the embodiment shown in FIG10 , by introducing at least one of the Runge-Kutta method and a numerical approximation method, a second-type analytical solution can be successfully obtained. Because lever-related constraints are introduced into the state estimation equation, the second-type analytical solution obtained from the state estimation equation can also reflect these lever-related constraints. Therefore, using the second-type analytical solution for estimating state quantities helps eliminate lever-related errors, thereby improving the accuracy of state estimation and, consequently, the relative positioning accuracy between the second device and the first device.

[0220] In some optional embodiments of the present disclosure, a state quantity, first data, and second data may be obtained. An estimated state quantity may be determined based on the state quantity, the first data, the second data, and a state estimation equation that conforms to predetermined lever arm constraints. In this manner, by introducing lever arm-related constraints into the state estimation equation, the reliability of the estimated state quantity may be ensured.

[0221] After determining the estimated state quantity, in the example where the estimated state quantity is used as the prior state quantity, the prior state quantity can be corrected to obtain the corrected state quantity. In the correction stage, the discrete-time error state transfer matrix can be determined based on the first-class analytical solution, and the discrete-time error state transfer matrix can be used. In this way, the reliability of the discrete-time error state transfer matrix can be guaranteed, which is conducive to ensuring the reliability of the corrected state quantity. The corrected relative position and corrected relative posture in the corrected state quantity can be used as the relative positioning result between the second device and the first device. Therefore, the use of the embodiments of the present disclosure is conducive to improving the relative positioning accuracy between the second device and the first device.

[0222] Any of the state estimation methods provided in the embodiments of the present disclosure can be executed by any appropriate device with data processing capabilities, including but not limited to: a terminal device and a server. Alternatively, any of the state estimation methods provided in the embodiments of the present disclosure can be executed by a processor, such as a processor that executes any of the state estimation methods mentioned in the embodiments of the present disclosure by invoking corresponding instructions stored in a memory. This will not be further described below.

[0223] Exemplary devices

[0224] As shown in FIG11 , it is a structural diagram of an apparatus for state estimation provided by some exemplary embodiments of the present disclosure. The apparatus shown in FIG11 may include: a first acquisition module 1110, for acquiring state quantities, first data collected by a first inertial measurement unit provided on a first device, and second data collected by a second inertial measurement unit provided on a second device; wherein the state quantities include: target parameters, the relative position and relative attitude between the second device and the first device, the angular velocity bias and acceleration bias of the first inertial measurement unit, and the angular velocity bias and acceleration bias of the second inertial measurement unit, and the target parameters include at least one of the following two: the relative velocity between the second device and the first device, the relative position and relative attitude between the second device and the first device in the inertial coordinate system, and the relative position and relative attitude between the second device and the first device in the inertial coordinate system. The projection result obtained by projecting the relative speed onto the reference coordinate system, where the reference coordinate system is the coordinate system corresponding to one of the first inertial measurement unit and the second inertial measurement unit; a second acquisition module 1120 is used to obtain a state estimation equation that meets the predetermined lever arm constraint condition; wherein the predetermined lever arm constraint condition is defined by at least one of the following first equation and second equation, the first equation defines the relative speed between the second device and the first device, and the second equation defines the relative position between the second device and the first device; a first determination module 1130 is used to determine the estimated state quantity based on the state quantity, the first data, the second data and the state estimation equation.

[0225] In some optional embodiments of the present disclosure, as shown in FIG12 , the apparatus provided by the embodiment of the present disclosure may further include: a third acquisition module 1210 for acquiring a state covariance matrix of an error state quantity corresponding to a state quantity, a first type of analytical solution determined based on preset definition information, and observation data obtained by observing the first device and the second device through the target sensor; wherein the preset definition information refers to: definition information of parameters in the discrete time error state transfer matrix, the first type of analytical solution refers to: analytical solution of parameters in the discrete time error state transfer matrix, and the type of the target sensor is different from the inertial measurement unit; a second determination module 1220 for determining the error state quantity based on the state quantity, the prior state quantity, and the first type of analytical solution determined based on the preset definition information. , the first data, the second data and the first type of analytical solution, to determine the discrete-time error state transfer matrix; the fourth acquisition module 1230 is used to obtain the noise Jacobian matrix and the continuous-time noise covariance matrix; the third determination module 1240 is used to determine the discrete-time noise covariance matrix based on the discrete-time error state transfer matrix, the noise Jacobian matrix and the continuous-time noise covariance matrix; the fourth determination module 1250 is used to determine the priori state covariance matrix based on the discrete-time error state transfer matrix, the discrete-time noise covariance matrix and the state covariance matrix; the first correction module 1260 is used to correct the priori state quantity based on the priori state covariance matrix and the observation data.

[0226] In some optional embodiments of the present disclosure, as shown in FIG13 , the apparatus provided by the embodiment of the present disclosure may further include: a fifth acquisition module 1310 for acquiring a first type of analytical solution determined based on preset definition information, and observation data obtained by observing the first device and the second device through the target sensor; wherein the preset definition information refers to: definition information of parameters in the discrete time error state transfer matrix, the first type of analytical solution refers to: analytical solution of parameters in the discrete time error state transfer matrix, and the type of the target sensor is different from the inertial measurement unit; a fifth determination module 1320 for determining the first type of analytical solution based on the state quantity, the prior state quantity, the first data, the second data and the first type of analytical solution. solution, and determine the discrete-time error state transfer matrix; a sixth acquisition module 1330, used to obtain the noise Jacobian matrix and the continuous-time noise covariance matrix; a sixth determination module 1340, used to determine the discrete-time noise covariance matrix based on the discrete-time error state transfer matrix, the noise Jacobian matrix and the continuous-time noise covariance matrix; a seventh determination module 1350, used to determine the target Jacobian matrix of the error state quantity corresponding to the observed data, the first data and the second data relative to the state quantity; a second correction module 1360, used to correct the prior state quantity based on the discrete-time noise covariance matrix, the target Jacobian matrix, the observed data, the first data and the second data.

[0227] In some optional embodiments of the present disclosure, as shown in Figures 14-1 and 14-2, the device provided by the embodiments of the present disclosure may further include: a seventh acquisition module 1410, for acquiring a first-class analytical solution determined based on preset definition information; wherein the preset definition information refers to: definition information of parameters in the discrete-time error state transfer matrix, and the first-class analytical solution refers to: an analytical solution of parameters in the discrete-time error state transfer matrix; the device provided by the embodiments of the present disclosure may satisfy one of the following: the estimated state quantity is used as a priori state quantity, as shown in Figure 14-1, the device also includes: a third correction module 1420, for correcting the priori state quantity based on the state quantity, the priori state quantity, the first data, the second data and the first-class analytical solution; as shown in Figure 14-2, the device may further include: a fourth correction module 1430, for acquiring the priori state quantity, and correcting the priori state quantity based on the priori state quantity, the estimated state quantity, the first data, the second data and the first-class analytical solution, wherein at least part of the estimated state quantity is used as the observation data.

[0228] In some optional embodiments of the present disclosure, as shown in FIG15 , the third correction module 1420 includes:

[0229] The first acquisition submodule 1510 is used to obtain the state covariance matrix and observation data of the error state quantity corresponding to the state quantity;

[0230] A first determining submodule 1520 is configured to determine a discrete-time error state transfer matrix based on the state quantity, the priori state quantity, the first data, the second data, and the first type of analytical solution;

[0231] The second acquisition submodule 1530 acquires the noise Jacobian matrix and the continuous-time noise covariance matrix;

[0232] A second determining submodule 1540 is configured to determine a discrete-time noise covariance matrix based on the discrete-time error state transfer matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix;

[0233] A third determining submodule 1550 is configured to determine a priori state covariance matrix based on the discrete-time error state transfer matrix, the discrete-time noise covariance matrix, and the state covariance matrix;

[0234] The first correction submodule 1560 is used to correct the prior state quantity based on the prior state covariance matrix and the observation data.

[0235] In some optional embodiments of the present disclosure, as shown in FIG16 , the third correction module 1420 includes:

[0236] The third acquisition submodule 1610 is used to acquire observation data;

[0237] A fourth determining submodule 1620 is configured to determine a discrete-time error state transfer matrix based on the state quantity, the priori state quantity, the first data, the second data, and the first type of analytical solution;

[0238] The fourth acquisition submodule 1630 is used to obtain the noise Jacobian matrix and the continuous-time noise covariance matrix;

[0239] a fifth determining submodule 1640 , configured to determine a discrete-time noise covariance matrix based on the discrete-time error state transfer matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix;

[0240] A sixth determining submodule 1650 is configured to determine a target Jacobian matrix of an error state quantity corresponding to the observed data, the first data, and the second data relative to the state quantity;

[0241] The second correction submodule 1660 is used to correct the prior state quantity based on the discrete-time noise covariance matrix, the target Jacobian matrix, the observation data, the first data, and the second data.

[0242] In some optional embodiments of the present disclosure, the parameter located in the i-th row and j-th column of the discrete-time error state transfer matrix is ​​represented by Φ ij (t, t0), then the first type of analytical solution may include at least one of the following: Φ12 (t,t0)=C′(t)(t-t0)

[0243] If i = 3, and j = 1, 2, 6, 7, Φ 3j (t,t0)=03

[0244] If i>3, and i=j, Φ ij (t,t0)=I3

[0245] If i>3 and i≠j, Φ ij (t,t0)=03

[0246] in,

[0247] In some optional embodiments of the present disclosure, the discrete-time noise covariance matrix is ​​expressed as Q k,k-1 , the discrete-time noise covariance matrix is ​​calculated as follows:

[0248] in, express The discrete-time error state transfer matrix between time instant k and time instant k-1, express The noise Jacobian matrix between time instant and time instant k-1, Q c represents the continuous-time noise covariance matrix.

[0249] In some optional embodiments of the present disclosure, as shown in Figure 17, any one of the second determination module 1220, the fifth determination module 1320, the first determination submodule 1520, and the fourth determination submodule 1620 may include: a fifth acquisition submodule 1710, for obtaining a processing result obtained by processing the first type of analytical solution according to a preset method; wherein the preset method includes at least one of the following: a Runge-Kutta method, a numerical approximation method; and a seventh determination submodule 1720, for determining a discrete-time error state transfer matrix based on the state quantity, the prior state quantity, the first data, the second data, and the processing result.

[0250] In some optional embodiments of the present disclosure, as shown in FIG18 , the apparatus provided by an embodiment of the present disclosure further includes: an eighth acquisition module 1810 for acquiring a continuous-time error state transfer matrix and observation data; an eighth determination module 1820 for determining a discrete-time error state transfer matrix based on the continuous-time error state transfer matrix; a ninth acquisition module 1830 for acquiring a noise Jacobian matrix and a continuous-time noise covariance matrix; a ninth determination module 1840 for determining a discrete-time noise covariance matrix based on the discrete-time error state transfer matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix; and a fifth correction module 1850 for correcting the estimated state quantity based on the discrete-time noise covariance matrix and the observation data; wherein the continuous-time error state transfer matrix is ​​represented by F, and the continuous-time error state transfer matrix is ​​calculated in the following manner:

[0251] In some optional embodiments of the present disclosure, the observation data includes at least one of the following: a preset observation relative position between the second device and the first device; data obtained by observing the first device and the second device through a target sensor, the type of the target sensor is different from the inertial measurement unit; first data collected by the first inertial measurement unit and second data collected by the second inertial measurement unit.

[0252] In some optional embodiments of the present disclosure, a first equation defines a relationship between a derivative of the relative velocity between the first device and the second device and a target difference, where the target difference refers to a difference obtained by removing the Coriolis acceleration and the drag acceleration from the reference relative acceleration. The reference relative acceleration is associated with a relative posture between the first device and the second device, a first corrected acceleration, and a second corrected acceleration. The first corrected acceleration is the difference between an acceleration measured by a first inertial measurement unit and an acceleration bias of the first inertial measurement unit. The second corrected acceleration is the difference between an acceleration measured by a second inertial measurement unit and an acceleration bias of the second inertial measurement unit. The Coriolis acceleration is associated with a corrected angular velocity and the relative velocity between the first device and the second device. The corrected angular velocity is the difference between an angular velocity measured by an inertial measurement unit corresponding to a reference coordinate system and the angular velocity bias of the inertial measurement unit. The drag acceleration is associated with the corrected angular velocity, the relative position between the first device and the second device, and a derivative of the corrected angular velocity.

[0253] In some optional embodiments of the present disclosure, the inertial measurement unit corresponding to the reference coordinate system is a first inertial measurement unit, and the first equation includes:

[0254] In some optional embodiments of the present disclosure, the inertial measurement unit corresponding to the reference coordinate system is a first inertial measurement unit, and the second equation includes:

[0255] It is the projection result obtained by projecting the relative velocity between the second device and the first device in the inertial coordinate system onto the reference coordinate system.

[0256] In some optional embodiments of the present disclosure, as shown in Figure 19, the first determination module 1130 includes: a sixth acquisition submodule 1910, used to obtain a second type of analytical solution determined based on the state estimation equation and a preset method; wherein the preset method includes at least one of the following: a Runge-Kutta method, a numerical approximation method, and the second type of analytical solution refers to: an analytical solution of a parameter in the estimated state quantity; an eighth determination submodule 1920, used to determine the estimated state quantity based on the state quantity, the first data, the second data and the second type of analytical solution.

[0257] In the device of the present disclosure, the various optional embodiments, optional implementation methods and optional examples disclosed above can be flexibly selected and combined as needed to achieve corresponding functions and effects, and the present disclosure does not list them one by one.

[0258] Exemplary electronic devices

[0259] FIG20 illustrates a block diagram of an electronic device according to an embodiment of the present disclosure. The electronic device 2000 includes one or more processors 2010 and a memory 2020 .

[0260] The processor 2010 may be a central processing unit (CPU) or other forms of processing units having data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device 2000 to perform desired functions.

[0261] The memory 2020 may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. Volatile memory may include, for example, random access memory (RAM) and / or cache memory. Non-volatile memory may include, for example, read-only memory (ROM), a hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 2010 may execute the one or more computer program instructions to implement the methods of the various embodiments of the present disclosure described above and / or other desired functions.

[0262] In one example, the electronic device 2000 may further include an input device 2030 and an output device 2040, which are interconnected via a bus system and / or other forms of connection mechanisms. The input device 2030 may also include, for example, a keyboard, a mouse, etc. The output device 2040 may output various information to the outside, and may include, for example, a display, a speaker, a printer, a communication network, and remote output devices connected thereto.

[0263] Of course, for simplicity, FIG20 only shows some of the components related to the present disclosure in the electronic device 2000, omitting components such as buses, input / output interfaces, etc. In addition, the electronic device 2000 may further include other appropriate components according to specific application scenarios.

[0264] Exemplary computer program products and computer-readable storage media

[0265] In addition to the above-mentioned methods and devices, an embodiment of the present disclosure may also be a computer program product, which includes computer program instructions that, when executed by a processor, enable the processor to perform the steps of the method according to various embodiments of the present disclosure described in the above-mentioned "Exemplary Method" section of this specification.

[0266] The computer program product may be written in any combination of one or more programming languages ​​to implement the operations of the disclosed embodiments, including object-oriented programming languages ​​such as Java, C++, and conventional procedural programming languages ​​such as C or similar programming languages. The program code may be executed entirely on the user's computing device, partially on the user's computing device, as a stand-alone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0267] In addition, an embodiment of the present disclosure may also be a computer-readable storage medium having computer program instructions stored thereon, which, when executed by a processor, enables the processor to execute the steps in the method according to various embodiments of the present disclosure described in the above "Exemplary Method" section of this specification.

[0268] The computer-readable storage medium can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can, for example, include but is not limited to a system, device or component of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination thereof. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof.

[0269] The basic principles of the present disclosure have been described above in conjunction with specific embodiments. However, the advantages, strengths, and effects mentioned in this disclosure are merely illustrative and non-limiting, and should not be construed as necessarily possessed by each embodiment of the present disclosure. The specific details disclosed above are provided for illustrative purposes and to facilitate understanding, and are not intended to be limiting. The above details do not limit the present disclosure to necessarily being implemented using the above specific details.

[0270] Those skilled in the art may make various changes and modifications to the present disclosure without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present disclosure and their equivalents, the present disclosure is intended to include these modifications and variations.

Claims

1. A method for state estimation, comprising: Obtaining state quantities, first data collected by a first inertial measurement unit disposed on a first device, and second data collected by a second inertial measurement unit disposed on a second device; wherein, the state quantities include: target parameters, the relative position and relative attitude between the second device and the first device, the angular velocity bias and acceleration bias of the first inertial measurement unit, and the angular velocity bias and acceleration bias of the second inertial measurement unit, and the target parameters include at least one of the following two: the relative velocity between the second device and the first device, and the projection result obtained by projecting the relative velocity between the second device and the first device in an inertial coordinate system onto a reference coordinate system, and the reference coordinate system is the coordinate system corresponding to one of the first inertial measurement unit and the second inertial measurement unit; Obtaining a state estimation equation that meets a predetermined lever arm constraint condition; wherein, the predetermined lever arm constraint condition is defined by at least one of the following first equation and second equation, the first equation defines the relative velocity between the second device and the first device, and the second equation defines the relative position between the second device and the first device; Determining an estimated state quantity based on the state quantities, the first data, the second data, and the state estimation equation.

2. The method according to claim 1, wherein The method further comprises: Obtaining a first type of analytical solution determined based on preset definition information; wherein, the preset definition information refers to the definition information of the parameters in the discrete-time error state transition matrix, and the first type of analytical solution refers to the analytical solution of the parameters in the discrete-time error state transition matrix; The method further comprises at least one of the following: Correcting a prior state quantity based on the state quantities, the prior state quantity, the first data, the second data, and the first type of analytical solution, wherein the estimated state quantity serves as the prior state quantity; Obtaining a prior state quantity, Correcting the prior state quantity based on the prior state quantity, the estimated state quantity, the first data, the second data, and the first type of analytical solution, wherein at least part of the estimated state quantity serves as observation data.

3. The method according to claim 2, wherein, The correcting the prior state quantity based on the state quantities, the prior state quantity, the first data, the second data, and the first type of analytical solution comprises: Obtaining the state covariance matrix of the error state quantity corresponding to the state quantities and observation data; Determining the discrete-time error state transition matrix based on the state quantities, the prior state quantity, the first data, the second data, and the first type of analytical solution; Obtaining a noise Jacobian matrix and a continuous-time noise covariance matrix; Determining a discrete-time noise covariance matrix based on the discrete-time error state transition matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix; Determining a prior state covariance matrix based on the discrete-time error state transition matrix, the discrete-time noise covariance matrix, and the state covariance matrix; Based on the prior state covariance matrix and the observation data, correct the prior state quantity.

4. The method according to claim 2, wherein, The correcting the prior state quantity based on the state quantity, the prior state quantity, the first data, the second data, and the first type of analytical solution includes: Obtain observation data; Based on the state quantity, the prior state quantity, the first data, the second data, and the first type of analytical solution, determine the discrete-time error state transition matrix; Obtain the noise Jacobian matrix and the continuous-time noise covariance matrix; Based on the discrete-time error state transition matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix, determine the discrete-time noise covariance matrix; Determine the target Jacobian matrix of the observation data, the first data, and the second data with respect to the error state quantity corresponding to the state quantity; Based on the discrete-time noise covariance matrix, the target Jacobian matrix, the observation data, the first data, and the second data, correct the prior state quantity.

5. The method according to any one of claims 2 to 4, wherein The parameter in the \(i\)-th row and \(j\)-th column of the discrete-time error state transition matrix is denoted as \(\varPhi\) ij (t, t0), then the first type of analytical solution includes at least one of the following items:(t, t0) = C′(t)(t - t0) \(\varPhi\) 12 (t, t0) = C′(t)(t - t0) ​ If i = 3 and j = 1, 2, 6, 7, Φ 3j (t, t0)) = 03 If i > 3 and i = j, Φ ij (t, t0) = I3 If i > 3 and i ≠ j, Φ ij (t, t0) = 03 Among them, 6. The method according to claim 3 or 4, wherein The discrete-time noise covariance matrix is denoted as Q k,k-1 , and the discrete-time noise covariance matrix is calculated in the following manner: Among them, Φ(τ) represents the discrete-time error state transition matrix between the τ-th moment and the (k - 1)-th moment, G(τ) represents the noise Jacobian matrix between the τ-th moment and the (k - 1)-th moment, and Q c represents the continuous-time noise covariance matrix.

7. The method according to claim 3 or 4, wherein The determining the discrete-time error state transition matrix based on the state quantity, the prior state quantity, the first data, the second data, and the first type of analytical solution includes: Obtain the processing result obtained by processing the first type of analytical solution according to a preset method; wherein the preset method includes at least one of the following: Runge-Kutta method, numerical approximation method; Based on the state quantity, the prior state quantity, the first data, the second data, and the processing result, determine the discrete-time error state transition matrix.

8. The method according to claim 1, wherein The method further includes: Obtain the continuous-time error state transition matrix and the observation data; Based on the continuous-time error state transition matrix, determine the discrete-time error state transition matrix; Obtain the noise Jacobian matrix and the continuous-time noise covariance matrix; Based on the discrete-time error state transition matrix, the noise Jacobian matrix, and the continuous-time noise covariance matrix, determine the discrete-time noise covariance matrix; Based on the discrete-time noise covariance matrix and the observation data, correct the estimated state quantity; Wherein, the continuous-time error state transition matrix is denoted as F, and the continuous-time error state transition matrix is calculated in the following manner:

9. The method according to any one of claims 3, 4 and 8, wherein The observation data includes at least one of the following: The preset observed relative position between the second device and the first device; Data obtained by observing the first device and the second device through a target sensor, where the type of the target sensor is different from that of the inertial measurement unit; The first data collected by the first inertial measurement unit and the second data collected by the second inertial measurement unit.

10. The method according to any one of claims 1-4 and 8, wherein, The first equation defines the relationship between the derivative result of the relative velocity between the first device and the second device and the target difference, where the target difference is the difference obtained by removing the Coriolis acceleration and the transport acceleration from the reference relative acceleration. Herein, the reference relative acceleration is related to the relative attitude between the first device and the second device, the first correction acceleration, and the second correction acceleration. The first correction acceleration is the difference between the acceleration measured by the first inertial measurement unit and the acceleration bias of the first inertial measurement unit. The second correction acceleration is the difference between the acceleration measured by the second inertial measurement unit and the acceleration bias of the second inertial measurement unit. The Coriolis acceleration is related to the correction angular velocity and the relative velocity between the first device and the second device. The correction angular velocity is the difference between the angular velocity measured by the inertial measurement unit corresponding to the reference coordinate system and the angular velocity bias of the inertial measurement unit. The transport acceleration is related to the correction angular velocity, the relative position between the first device and the second device, and the derivative result of the correction angular velocity.

11. According to the method described in any one of claims 1-4 and 8, wherein, The inertial measurement unit corresponding to the reference coordinate system is the first inertial measurement unit, and the first equation includes:

12. According to the method according to any one of claims 1-4 and 8, wherein, The inertial measurement unit corresponding to the reference coordinate system is the first inertial measurement unit, and the second equation includes: Among them, It represents the projection result obtained by projecting the relative velocity between the second device and the first device in the inertial coordinate system onto the reference coordinate system.

13. According to the method as claimed in any one of claims 1-4 and 8, wherein Determining the estimated state quantity based on the state quantity, the first data, the second data, and the state estimation equation includes: Obtaining a second type of analytical solution determined based on the state estimation equation and a preset method; wherein the preset method includes at least one of the following: Runge-Kutta method, numerical approximation method, and the second type of analytical solution refers to the analytical solution of the parameters in the estimated state quantity; Based on the state quantity, the first data, the second data, and the second type of analytical solution, determining the estimated state quantity.

14. An electronic device, comprising: A memory for storing a computer program product; A processor for executing the computer program product stored in the memory, and when the computer program product is executed, implementing the method for state estimation according to any one of claims 1 to 13 above.

15. A computer-readable storage medium, on which computer program instructions are stored, and when the computer program instructions are executed by a processor, implementing the method for state estimation according to any one of claims 1 to 13 above.

Images