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

Abstract

The invention provides a state estimation method and device, electronic equipment and a storage medium. According to the specific implementation scheme, the method comprises the following steps: acquiring a state quantity, first data acquired by a first inertial measurement unit arranged on first equipment, and second data acquired by a second inertial measurement unit arranged on second equipment; the state quantity comprises a target parameter, a relative position and a relative attitude between the second equipment and the first equipment, 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; the target parameter comprises at least one of the following two: the relative speed between the second equipment and the first equipment, and a projection result obtained by projecting the relative speed between the second equipment and the first equipment under an inertial coordinate system to a reference coordinate system; obtaining a state estimation equation meeting a predetermined lever arm constraint condition; an estimated state quantity is determined based on the state quantity, the first data, the second data, and a state estimation equation.

Description

Technical Field

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

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

[0003] According to one aspect of the present disclosure, there is provided a method for state estimation, including: obtaining a state quantity, 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 quantity includes: 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 includes 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 a 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 a coordinate system corresponding to one of the first inertial measurement unit and the second inertial measurement unit; obtaining a state estimation equation that conforms to 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 a relative velocity between the second device and the first device, and the second equation defines a 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.

[0004] According to another aspect of the present disclosure, there is provided an apparatus for state estimation, including: a first acquisition module configured to acquire 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, relative position and relative attitude between the second device and the first device, angular velocity bias and acceleration bias of the first inertial measurement unit, and angular velocity bias and acceleration bias of the second inertial measurement unit, and the target parameters include at least one of the following two: 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, and 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 configured to acquire 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; a first determination module configured to determine an estimated state quantity based on the state quantities, the first data, the second data, and the state estimation equation.

[0005] According to still another aspect of the present disclosure, there is provided a computer-readable storage medium storing a computer program for executing the above method for state estimation.

[0006] According to yet another aspect of the present disclosure, there is provided an electronic device, including: 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 method for state estimation. Description of the Drawings

[0007] Figure 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.

[0008] Figure 2 is a flowchart of a method for state estimation provided in some exemplary embodiments of the present disclosure.

[0009] Figure 3 is a flowchart of a method for correcting a prior state quantity provided in some exemplary embodiments of the present disclosure.

[0010] Figure 4-1 is a schematic diagram of an application scenario provided in some exemplary embodiments of the present disclosure.

[0011] Figure 4-2 is a schematic diagram of an application scenario provided in some other exemplary embodiments of the present disclosure.

[0012] Figure 4-3 It is a schematic diagram of an application scenario of some exemplary embodiments of the present disclosure.

[0013] Figure 5 It is a schematic flowchart of a method for correcting a prior state quantity provided by some other exemplary embodiments of the present disclosure.

[0014] Figure 6 It is a schematic flowchart of a method for determining a discrete-time error state transition matrix provided by some exemplary embodiments of the present disclosure.

[0015] Figure 7 It is a schematic flowchart of a method for correcting a prior state quantity provided by some other exemplary embodiments of the present disclosure.

[0016] Figure 8-1 It is a schematic flowchart of a method for correcting a prior state quantity provided by some other exemplary embodiments of the present disclosure.

[0017] Figure 8-2 It is a schematic flowchart of a method for correcting a prior state quantity provided by some other exemplary embodiments of the present disclosure.

[0018] Figure 9 It is a schematic flowchart of a method for correcting an estimated state quantity provided by some exemplary embodiments of the present disclosure.

[0019] Figure 10 It is a schematic flowchart of a method for determining an estimated state quantity provided by some exemplary embodiments of the present disclosure.

[0020] Figure 11 It is a schematic diagram of the structure of a device for state estimation provided by some exemplary embodiments of the present disclosure.

[0021] Figure 12 It is a schematic diagram of a module involved in the correction of a prior state quantity in some exemplary embodiments of the present disclosure.

[0022] Figure 13 It is a schematic diagram of a module involved in the correction of a prior state quantity in some other exemplary embodiments of the present disclosure.

[0023] Figure 14-1 It is a schematic diagram of a module involved in the correction of a prior state quantity in some other exemplary embodiments of the present disclosure.

[0024] Figure 14-2 It is a schematic diagram of a module involved in the correction of a prior state quantity in some other exemplary embodiments of the present disclosure.

[0025] Figure 15Schematic diagram of a module related to the correction of prior state quantities in some exemplary embodiments of the present disclosure.

[0026] Figure 16 Schematic diagram of a module related to the correction of prior state quantities in some exemplary embodiments of the present disclosure.

[0027] Figure 17 Schematic diagram of the structure of any one of the second determination module, the fifth determination module, the first determination sub-module, and the fourth determination sub-module in some exemplary embodiments of the present disclosure.

[0028] Figure 18 Schematic diagram of a module related to the correction of estimated state quantities in some exemplary embodiments of the present disclosure.

[0029] Figure 19 Schematic diagram of the structure of the first determination module in some exemplary embodiments of the present disclosure.

[0030] Figure 20 Structural diagram of an electronic device provided in some exemplary embodiments of the present disclosure. Detailed implementation manners

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

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

[0033] Exemplary Overview

[0034] In some application scenarios, relative positioning of two devices is required.

[0035] For example, one of the two devices can be a head-mounted display device, and the other can be a movable platform on which the head-mounted display device is located. Also, for example, one of the two devices can be a head-mounted display device, and the other can be an adapter device communicatively connected to the head-mounted display device. The head-mounted display device can also be referred to as a Head-Mounted Display (HMD) or a head-mounted display. The head-mounted display device can be presented in the form of glasses, helmets, etc. The head-mounted display device can include, but is not limited to, Augmented Reality (AR) glasses, Virtual Reality (VR) glasses, etc. The movable platform can include, but is not limited to, vehicles, ships, airplanes, trains, etc. By relatively positioning the head-mounted display device with the movable platform or with the adapter device, the relative pose between the head-mounted display device and the movable platform or with the adapter device can be obtained. Referring to the relative pose between the head-mounted display device and the movable platform or with 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 achieving a 6dof (degree of freedom) display effect.

[0036] Again, for example, one of the two devices can be a drone, and the other can be a movable platform to which the drone is to land. By relatively positioning the drone with the movable platform, the relative pose between the drone and the movable platform can be obtained. Referring to the relative pose between the drone and the movable platform, the motion parameters of each of the drone and the movable platform can be controlled so that the drone can successfully land on the movable platform.

[0037] Also, for example, one of the two devices can be a first aircraft flying in the air and in need of refueling, and the other can be a second aircraft flying in the air and used for fuel supply. By relatively positioning the first aircraft with the second aircraft, the relative pose between the first aircraft and the second aircraft can be obtained. Referring to the relative pose between the first aircraft and the second aircraft, the flight parameters of each of the first aircraft and the second aircraft can be controlled so that the second aircraft can successfully refuel the first aircraft in the air.

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

[0039] Exemplary Method

[0040] To effectively achieve relative positioning between two devices, embodiments of the present disclosure provide a method for state estimation. For the sake of easy distinction, hereinafter, one of the two devices that needs to perform state estimation can be referred to as the first device, and the other can be referred to as the second device.

[0041] In some alternative embodiments of the present disclosure, the first device may be a movable platform where the head-mounted display device is located, and the second device may be the head-mounted display device.

[0042] In some other alternative embodiments of the present disclosure, the first device may be a portable device communicatively connected to the head-mounted display device, and the second device may be the head-mounted display device. The portable device and the head-mounted display device can be used in an immovable scenario, or the portable device can be removably fixed to the movable platform where the head-mounted display device is located, and the portable device and the head-mounted display device are used in cooperation. The removable fixing method may include, but is not limited to, screwing, clamping, plugging, etc.

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

[0044] When used in the scenario of a movable platform, since the portable device is relatively fixed to the movable platform, 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 movable platform can be achieved on this basis.

[0045] Of course, the types of the first device and the second device can also be interchanged. For example, the first device may be the head-mounted display device, and the second device may be the movable platform.

[0046] The first device and the second device may be respectively provided with an Inertial Measurement Unit (IMU). For the convenience of distinction, the inertial measurement unit provided in the first device may be referred to as the first inertial measurement unit, and the inertial measurement unit provided in the second device may be referred to as the second inertial measurement unit. It can be understood that the inertial measurement unit is a device that uses an accelerometer and a gyroscope to measure the angular velocity and acceleration 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.

[0047] It should be noted that the system including the first device and the second device can be regarded as a dual-IMU system. The dual-IMU system involves the first inertial measurement unit provided in the first device and the second inertial measurement unit provided in the second device. Since the first inertial measurement unit and the second inertial measurement unit cannot coincide in space, there is an arm between the first inertial measurement unit and the second inertial measurement unit. Specifically, reference can be made to Figure 1, this will bring about lever-arm effects, generate lever-arm errors, and thus affect 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, without processing the lever-arm error, the lever-arm error can be 1 meter.

[0048] In view of this, in the embodiments of the present disclosure, a state estimation equation considering the lever-arm error is provided. The state estimation equation may refer to an equation for determining the state of the relative motion between the second device and the first device based on observations and measurements. Constraints related to the lever-arm can be introduced into the state estimation equation. Combining the data collected by the first inertial measurement unit and the second inertial measurement unit respectively, and the state estimation equation, state estimation can be performed. Using the estimated state, relative positioning between the second device and the first device can be achieved. In this way, the lever-arm error can be excluded as much as possible, the accuracy of state estimation can be improved, and thus the relative positioning accuracy between the second device and the first device can be improved.

[0049] As Figure 2 shown, it is a schematic flowchart of a method for state estimation provided by some exemplary embodiments of the present disclosure. Figure 2 The method shown may include step 210, step 220, and step 230.

[0050] Step 210, obtain 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 the inertial coordinate system onto the 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.

[0051] In some alternative embodiments of the present disclosure, both the first inertial measurement unit and the second inertial measurement unit may 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 centroid 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 centroid or other position point of the second inertial measurement unit as the origin. In an alternative example, the first coordinate system may be Figure 1 where the origin is O m and the three coordinate axes are X m 、Y m, Z m A three-dimensional coordinate system, and the second coordinate system can be Figure 1 with the origin at O b , and the three coordinate axes are respectively X b , Y b , Z b of the three-dimensional coordinate system.

[0052] It should be noted that the reference coordinate system can be the first coordinate system or the 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 velocity and relative attitude between the second device and the first device can be understood in a similar manner and will not be elaborated here. Additionally, if the reference coordinate system is the first coordinate system, the relative velocity between the second device and the first device in the inertial coordinate system can be understood as: the velocity difference obtained by subtracting the velocity of the first device in the inertial coordinate system from the velocity of the second device in the inertial coordinate system. If the reference coordinate system is the second coordinate system, the relative velocity between the second device and the first device in the inertial coordinate system can be understood as: the velocity difference obtained by subtracting the velocity of the second device in the inertial coordinate system from the velocity of the first device in the inertial coordinate system.

[0053] In some alternative embodiments of the present disclosure, state estimation can be performed at regular time intervals using a state estimator. When implementing the method of the embodiments of the present disclosure, various state estimators can be used. For example, the state estimator can be either a Kalman filter or other types of state estimators. If the state estimator used in the embodiments 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 embodiments of the present disclosure is other types of state estimators, the state quantity obtained in step 210 can include: the current latest state quantity, or can include: the current latest state quantity and several historical state quantities.

[0054] In some alternative embodiments of the present disclosure, the first data obtained in step 210 may include: the latest angular velocity data and acceleration data that the first inertial measurement unit can obtain before the current state estimation, as well as the historical angular velocity data and historical acceleration data obtained by the first inertial measurement unit before the current state estimation. The type of the second data obtained in step 210 can be referred to the relevant introduction of the first data and will not be elaborated here.

[0055] In some alternative 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:

[0056] v' = v + ω1 × p

[0057] where ω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 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.

[0059] As introduced 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 magnitude of the lever-arm effect can be proportional to the target cross-product result. The target cross-product result is the result obtained by taking the 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. Only 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, can the lever-arm effect be ignored. Obviously, the magnitude 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 taking the derivative of 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 magnitude 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, the first equation that defines the relative velocity between the second device and the first device may be introduced into the state estimation equation. In other embodiments, the second equation that defines the relative position between the second device and the first device may be introduced. In this way, it can be considered that the lever-arm related constraints are introduced into the state estimation equation, and the state estimation equation meets the predetermined lever-arm constraint condition.

[0061] Step 230, determine 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 quantity obtained in step 210 only includes the current latest state quantity as an example, the estimated state quantity determined in step 230 may include: the estimated parameters corresponding to each parameter in the state quantity 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 quantity determined in step 230 may include: the estimated relative position corresponding to the relative position in the state quantity obtained in step 210, the estimated relative attitude corresponding to the relative attitude in the state quantity obtained in step 210, the estimated relative velocity corresponding to the relative velocity in the state quantity obtained in step 210, the estimated angular velocity bias corresponding to the angular velocity bias of the first inertial measurement unit in the state quantity obtained in step 210, the estimated acceleration bias corresponding to the acceleration bias of the first inertial measurement unit in the state quantity obtained in step 210, the estimated angular velocity bias corresponding to the angular velocity bias of the second inertial measurement unit in the state quantity obtained in step 210, and the estimated acceleration bias corresponding to the acceleration bias of the second inertial measurement unit in the state quantity obtained in step 210.

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

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

[0065] In the embodiments of the present disclosure, state estimation can be performed by combining the state quantity, the data collected by the first inertial measurement unit and the second inertial measurement unit respectively, and the state estimation equation that conforms to the predetermined lever arm constraint condition, so as to obtain the estimated state quantity. In this way, it is equivalent to considering the lever arm existing between the first inertial measurement unit and the second inertial measurement unit during the process of state estimation, which is beneficial to eliminating the lever arm error and obtaining the true relative velocity and relative attitude (the relative attitude can include the relative position and the relative attitude) between the second device and the first device. Therefore, by adopting the embodiments of the present disclosure, the accuracy of state estimation can be improved, and thus the relative positioning accuracy between the second device and the first device can be improved.

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

[0067] Optionally, the state variables of the state estimation equation can be expressed in the following form:

[0068]

[0069] Optionally, the state variables can be maintained all the time. When the estimated state variables determined in the embodiments of the present disclosure are used as prior state variables, the above-mentioned state variables can be estimated and used for subsequent required operations. When the estimated state variables determined in the embodiments of the present disclosure are used as observations, the required state variables can be selected as observation data. When the estimated state variables determined in the embodiments of the present disclosure are used as observations, the present disclosure does not limit the calculation method of the prior state variables, and it is only necessary to calculate the prior value or initial value of the state variables defined in the present disclosure.

[0070] In some alternative embodiments of the present disclosure, 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. The reference relative acceleration is associated with 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 associated with 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 associated with the correction angular velocity, the relative position between the first device and the second device, and the derivative result of the correction angular velocity.

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

[0072]

[0073] Wherein, represents the relative velocity between the second device and the first device, and the dot above any parameter represents the derivative operation, C() represents converting the parameter in () from the quaternion form to the rotation matrix form, represents the relative attitude between the second device and the first device, represents the second correction acceleration, represents the first correction acceleration, represents the reference relative acceleration, represents the Coriolis acceleration, represents the correction angular velocity, and can form the transport acceleration, represents the relative position between the second device and the first device.

[0074] It should be noted that the Coriolis acceleration is the acceleration that appears when there is relative motion between the first coordinate system and the second coordinate system. The Coriolis acceleration involves and related to the magnitude of the lever arm effect, and the transport acceleration is the acceleration caused by the motion of the reference coordinate system. The transport acceleration involves and Therefore, through the first equation, the constraints related to the lever arm can be effectively introduced.

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

[0076]

[0077]

[0078] wherein, 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. It should be noted that the meanings of other parameters involved in the second equation can be referred to the relevant descriptions of the meanings of the parameters involved in the first equation above, and will not be elaborated here.

[0079] It should be noted that, and can satisfy the following formula:

[0080]

[0081] Combined with and the specific formula involved in the first equation above, the specific formula of the second equation above can be derived. Since and are both related to the magnitude of the lever arm effect, through the second equation, the constraints related to the lever arm can also be effectively introduced.

[0082] Optionally, if the state estimation equation includes the first equation as described above, in addition to including the first equation, the state estimation equation may further include the following equation:

[0083]

[0084]

[0085]

[0086]

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

[0088]

[0089]

[0090]

[0091] where 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, represents the acceleration bias of the second inertial measurement unit.

[0092] Hereinafter, taking the estimated state quantity determined in the embodiments of the present disclosure as the prior state quantity as an example for explanation. It can be understood that the relevant description does not mean a limitation on the estimated state quantity determined by the present disclosure as an observation.

[0093] As Figure 3 shown, it is a schematic flowchart of a method for correcting a prior state quantity provided by some exemplary embodiments of the present disclosure. Figure 3 The method shown may include step 310, step 320, step 330, step 340, step 350, and step 360. Optionally, Figure 3 the method shown may be executed after step 230 of the present disclosure.

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

[0095] Optionally, the target sensor may be, for example, Figure 4-1 and Figure 4-2 the image sensor shown.

[0096] Step 310 may 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 the preset definition information, and the observation data; where 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.

[0097] Taking the case where the target parameter is the relative velocity between the second device and the first device as an example, the error state quantity corresponding to the state quantity may be expressed in the following form:

[0098]

[0099] Where can successively represent 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 quantity obtained in step 210. The state covariance matrix obtained in step 310 may include: the covariance between the error parameters in the error state quantity.

[0100] In some alternative embodiments of the present disclosure, based on the preset definition information, the analytical solution of the parameters in the discrete-time error state transition matrix may be derived by using the analytical method to obtain the first type of analytical solution. Additionally, the first type of analytical solution may be stored in a preset first storage area. In step 310, the first type of analytical solution may be obtained from the first storage area. It can be understood that the analytical solution is a concept corresponding to the numerical solution, and the analytical solution can be considered as a strict formula. Given any independent variable, substituting the independent variable into the formula for calculation can obtain the corresponding dependent variable.

[0101] Step 320, 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.

[0102] Step 330, obtaining the noise Jacobian matrix and the continuous-time noise covariance matrix.

[0103] In some alternative embodiments of the present disclosure, based on the state quantity, the prior state quantity, the first data, and the second data, the noise Jacobian matrix may be obtained through real-time calculation.

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

[0105] Q cQ(t) = En(t + τ)n(t) T (t)

[0106] Where, Q c (t) represents the continuous-time noise covariance matrix at time t, E[] represents the expectation of calculating the content in [], n(t + τ) represents the state noise quantity at t + τ, and n T (t) represents the transpose of the state noise quantity at time t.

[0107] Step 340: Determine the 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.

[0108] In some alternative embodiments of the present disclosure, 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:

[0109]

[0110] Where, Φ(τ) represents the discrete-time error state transition matrix between time and time k - 1, G(τ) represents the noise Jacobian matrix between time and time k - 1, and Q c represents the continuous-time noise covariance matrix.

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

[0112] Step 350: Determine the prior state covariance matrix based on the discrete-time error state transition matrix, the discrete-time noise covariance matrix, and the state covariance matrix.

[0113] In some alternative embodiments of the present disclosure, according to the discrete-time error state transition matrix, the discrete-time noise covariance matrix, and the state covariance matrix, the prior state covariance matrix can be obtained through real-time calculation. In an alternative example, the discrete-time error state transition matrix is denoted as Φ, the discrete-time noise covariance matrix is denoted as Q, and the state covariance matrix is denoted as P. The prior state covariance matrix can be calculated based on Φ, P, the transpose of Φ, Q, the state noise quantity mentioned above, and the transpose of Q.

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

[0115] In some alternative embodiments of the present disclosure, the prior state covariance matrix can be used to determine the Kalman gain, the observation data and the Kalman gain can be used to determine the posterior error, and by combining the prior state quantity with the posterior error, the correction of the prior state quantity can be achieved. The correction result of the prior state quantity can be used as the corrected state quantity. The corrected state quantity can include: correction parameters corresponding to each prior parameter in the prior state quantity. That is to say, the corrected state quantity can include: the corrected relative position corresponding to the prior relative position in the prior state quantity, and the corrected relative attitude corresponding to the prior relative attitude in the prior 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. Then, the relative positioning result can be output. Additionally, the corrected state quantity can be used as the latest state quantity.

[0116] Figure 3 In the illustrated embodiment, the first type of analytical solution determined based on the preset definition information can be used to determine the discrete-time error state transition matrix. In this way, it is beneficial to ensure the reliability of the determined discrete-time error state transition 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 better ensured. In this way, using the prior state covariance matrix and the observation data to correct the prior state quantity can better ensure the correction effect.

[0117] For example, referring to Figure 3 the method shown, an error state Kalman filter (ESKF) can be used for state estimation.

[0118] As Figure 5 shown, it is a schematic flowchart of a method for correcting a prior state quantity provided by some other exemplary embodiments of the present disclosure. Figure 5 The method shown can include step 510, step 520, step 530, step 540, step 550, and step 560. Optionally, Figure 5 the method shown can be executed after step 230 of the present disclosure.

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

[0120] Step 510 may also be to obtain a first type of analytical solution and observation data 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.

[0121] Step 520, 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.

[0122] Step 530, obtain the noise Jacobian matrix and the continuous-time noise covariance matrix.

[0123] Step 540, 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.

[0124] For example, the specific implementation manners of steps 510 to 540 may also refer to the relevant introductions to steps 310 to 340 in the above text, and will not be elaborated here.

[0125] Step 550, 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.

[0126] For example, the specific composition of the error state quantity corresponding to the state quantity may also refer to the relevant introduction to step 310, and will not be elaborated here.

[0127] In some alternative embodiments of the present disclosure, a first Jacobian matrix of the observation data with respect to the error state quantity corresponding to the state quantity may be determined, a second Jacobian matrix of the first data with respect to the error state quantity corresponding to the state quantity may be determined, and a third Jacobian matrix of the second data with respect to the error state quantity corresponding to the state quantity may be determined. The target Jacobian matrix may include the first Jacobian matrix, the second Jacobian matrix, and the third Jacobian matrix. Of course, in specific implementation, the observation data, the first data, and the second data may also be regarded as a whole, and a fourth Jacobian matrix of this whole with respect to the error state quantity corresponding to the state quantity may be calculated, and the fourth Jacobian matrix may be used as the target Jacobian matrix.

[0128] Step 560, 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.

[0129] In some alternative embodiments of the present disclosure, the posterior error may be determined based on the discrete-time noise covariance matrix, the target Jacobian matrix, the observation data, the first data, and the second data, and the prior state quantity and the posterior error may be combined to achieve the correction of the prior state quantity. The correction result of the prior state quantity may be used as the corrected state quantity.

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

[0131] Figure 5 In the illustrated embodiment, the first type of analytical solution determined based on the preset definition information can be used to determine the discrete-time error state transition matrix. In this way, it is beneficial to ensure the reliability of the determined discrete-time error state transition 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 better ensured. The discrete-time noise covariance matrix, the target Jacobian matrix, the observed data, the first data, and the second data can all be used for correcting the prior state quantity. In this way, the data used for correcting the prior state quantity is very rich. For example, it includes both the observed data and the data collected by the inertial measurement unit, which can better ensure the correction effect.

[0132] For example, referring to Figure 5 the method shown, state estimation can be performed using other state estimators besides the Kalman filter.

[0133] Referring to Figure 6 , a method for determining the discrete-time error state transition matrix is introduced, and this method can be used for Figure 3 and Figure 5 the method shown.

[0134] As Figure 6 shown, it is a schematic flowchart of a method for determining the discrete-time error state transition matrix provided by some exemplary embodiments of the present disclosure. Figure 6 The method shown can include step 610 and step 620. Optionally, the combination of step 610 and step 620 can be used as an alternative implementation of Figure 3 step 320. The combination of step 610 and step 620 can also be used as an alternative implementation of Figure 5 step 520.

[0135] Step 610, 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.

[0136] In some alternative embodiments of the present disclosure, both the Runge-Kutta method and the numerical approximation method can be used for processing angular velocity, acceleration, velocity, translation, etc. The Runge-Kutta method can include, but is not limited to, the third-order Runge-Kutta method, the fourth-order Runge-Kutta method, the fifth-order Runge-Kutta method, etc. It can be understood that the Runge-Kutta method is a high-precision single-step algorithm widely used in engineering. Given the derivative and initial value information of the equation, the complex process of solving the differential equation can be omitted using computer simulation technology. The numerical approximation method can include, but is not limited to, the zero-order approximation method, the first-order approximation method, the second-order approximation method, etc.

[0137] The zero-order approximation method can be understood as follows: Suppose it is necessary to integrate a certain parameter over a period of time. Then, the starting value of this parameter during this period can be used as the zero-order approximation result, or the final value of this parameter during this period can be used as the zero-order approximation result, or the average value of the starting value and the final value of this parameter during this period can be used as the zero-order approximation result, or the average value or weighted average value of the starting value, the intermediate value, and the final value of this parameter during this period can be used as the zero-order approximation result.

[0138] The first-order approximation method can be understood as follows: During the integration process, the angle can be processed in a uniform angular acceleration manner, the acceleration can be processed in a uniform jerk manner, the velocity can be processed in a uniform acceleration manner, the translation can be processed in a uniform velocity manner, etc.

[0139] The second-order approximation method can be understood as follows: Suppose it is necessary to integrate a certain parameter over a period of time. Then, integrate according to the formula A = a + bt + (1 / 2)ct2, where a, b, and c represent coefficients, and t represents time. If this parameter is angular velocity, and the starting moment of this period is the t0 moment, a can be the angular velocity at the t0 moment, b can be the angular acceleration at the t0 moment, and c can be the angular jerk at the t0 moment. Similarly, if this parameter is velocity, a can be the velocity at the t0 moment, b can be the acceleration at the t0 moment, and c can be the jerk at the t0 moment.

[0140] In some alternative 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, in the first type of analytical solution, there are analytical solutions corresponding to multiple parameters respectively. All the analytical solutions in the first type of analytical solution can be divided into two categories, namely, analytical solutions with high complexity and analytical solutions with low complexity. Suppose some analytical solutions only involve simple operation logics such as single integral or addition and subtraction, then such analytical solutions can be classified as analytical solutions with low complexity. Suppose some analytical solutions involve multiple integrals or other complex operation logics, then such analytical solutions can be classified as analytical solutions with high complexity. For the analytical solutions with high complexity, at least one of the Runge-Kutta method and the numerical approximation method can be introduced to simplify the analytical solution to obtain a simplified result of the analytical solution. For the analytical solutions with low complexity, there is no need to introduce the Runge-Kutta method and the numerical approximation method. In this way, the processing result can include: the simplified result of the analytical solution with high complexity, and the analytical solution with low complexity. The processing result can be regarded as the simplified result of the first type of analytical solution. Optionally, the processing result can be stored in a preset second storage area. In step 610, the processing result can be obtained from the second storage area.

[0141] Step 620: Determine the discrete-time error state transition matrix based on the state quantity, the prior state quantity, the first data, the second data, and the processing result.

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

[0143] It should be noted that since the first type of analytical solution is a strict formula obtained through derivation, the first type of analytical solution can very accurately reflect the operation logic required to obtain the discrete-time error state transition matrix. By introducing at least one of the Runge-Kutta method and the numerical approximation method to process the first type of analytical solution, the first type of analytical solution can be simplified to a certain extent. In this way, using the processing result obtained by processing the first type of analytical solution to determine the discrete-time error state transition matrix can, on the one hand, ensure the reliability of the calculated discrete-time error state transition matrix, and on the other hand, reduce the computational complexity, improve the computational speed, and improve the efficiency of obtaining the discrete-time error state transition matrix.

[0144] Of course, the implementation manner of determining the discrete-time error state transition 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 also be directly substituted into the first type of analytical solution for calculation to obtain the discrete-time error state transition matrix.

[0145] Some exemplary embodiments of the present disclosure provide a method for estimating a state quantity, which may include: 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 a discrete-time error state transition matrix, and the first type of analytical solution refers to an analytical solution of parameters in the discrete-time error state transition matrix.

[0146] For the specific implementation manners, reference may be made to the explanations in other embodiments of the present disclosure, which will not be elaborated herein.

[0147] The method may further include using the estimated state quantity as a prior state quantity, and 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.

[0148] The method may further include obtaining a prior state quantity, and 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, where at least part of the estimated state quantity is used as observed data.

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

[0150] Step 710: Obtain a first type of 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 transition matrix, and the first type of analytical solution refers to an analytical solution of parameters in the discrete-time error state transition matrix.

[0151] For example, referring to Figure 3 the relevant introduction in the embodiments shown, an analytical solution of parameters in the discrete-time error state transition matrix may be derived by using an analytical method based on the preset definition information to obtain the first type of analytical solution.

[0152] Step 720: Correct 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.

[0153] In some optional implementation manners of the present disclosure, as Figure 8-1 shown, step 720 may include:

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

[0155] Step 7203: Determine the discrete-time error state transition matrix based on the state quantity, prior state quantity, first data, second data, and the first type of analytical solution.

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

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

[0158] Step 7209: Determine the prior state covariance matrix based on the discrete-time error state transition matrix, discrete-time noise covariance matrix, and state covariance matrix.

[0159] Step 7211: Correct the prior state quantity based on the prior state covariance matrix and the observation data.

[0160] For example, Figure 8-1 For the specific implementation process and achievable effects of the illustrated implementation manner, refer to the relevant introduction of the illustrated embodiment in the foregoing text. Figure 3 Details are not described herein again.

[0161] In some other alternative implementation manners of the present disclosure, as Figure 8-2 illustrated, step 720 may include:

[0162] Step 7213: Obtain the observation data.

[0163] Step 7215: Determine the discrete-time error state transition matrix based on the state quantity, prior state quantity, first data, second data, and the first type of analytical solution.

[0164] Step 7217: Obtain the noise Jacobian matrix and the continuous-time noise covariance matrix.

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

[0166] Step 7221: Determine the target Jacobian matrix of the observation data, first data, and second data with respect to the error state quantity corresponding to the state quantity.

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

[0168] For example, Figure 8-2 For the specific implementation process and achievable effects of the illustrated implementation manner, refer to the relevant introduction of the illustrated embodiment in the foregoing text. Figure 5A relevant introduction to the illustrated embodiments is sufficient, and will not be elaborated here.

[0169] Figure 7 In the illustrated embodiments, since the first type of analytical solution is a strict formula obtained through derivation, using the first type of analytical solution for correcting the prior state quantity is beneficial to ensuring the correction effect.

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

[0171] Step 910, obtain the continuous-time error state transition matrix and the observation data.

[0172] The continuous-time error state transition matrix may be represented as F, and the continuous-time error state transition matrix is calculated in the following manner:

[0173]

[0174]

[0175]

[0176]

[0177] Wherein, 03 represents the 0 matrix, I3 represents the identity matrix, [×] represents the skew-symmetric matrix of calculating the parameters in [], 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.

[0178] Step 920, determine the discrete-time error state transition matrix based on the continuous-time error state transition matrix.

[0179] In some alternative embodiments of the present disclosure, according to the continuous-time error state transition matrix, the discrete-time error state transition matrix can be obtained through Taylor expansion. Assuming that the continuous-time error state transition matrix is represented by F and the discrete-time error state transition matrix is represented by Φ, the discrete-time error state transition matrix can be determined in the following manner:

[0180]

[0181] Among them, the specific number of terms taken for Taylor expansion can depend on the requirement for accuracy.

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

[0183]

[0184] Then, based on the formula in the previous paragraph, an analytical method can be used to derive the analytical solution of the parameters in the discrete-time error state transition matrix to obtain the first type of analytical solution. After that, the discrete-time error state transition matrix can be determined using the first type of analytical solution.

[0185] Step 930, obtain the noise Jacobian matrix and the continuous-time noise covariance matrix.

[0186] Step 940, determine the 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.

[0187] For example, the specific implementation manners of step 930 and step 940 can refer to the relevant introductions of step 330 and step 340 in the above text, and will not be elaborated here.

[0188] Step 950, correct the estimated state quantity based on the discrete-time noise covariance matrix and the observation data.

[0189] In some alternative embodiments of the present disclosure, the estimated state quantity can be used as the prior state quantity. Referring to Figure 3 the introduction in the illustrated embodiment, first determine the prior state covariance matrix based on the discrete-time error state transition matrix, the discrete-time noise covariance matrix, and the state covariance matrix, and then correct the prior state quantity based on the prior state covariance matrix and the observation data.

[0190] In some other alternative embodiments of the present disclosure, the estimated state quantity can be used as the prior state quantity. Referring to Figure 5As introduced in the illustrated embodiment, a target Jacobian matrix of the error state quantity corresponding to the state quantity of the observed data, the first data and the second data is determined, and the prior state quantity is corrected based on the discrete-time noise covariance matrix, the target Jacobian matrix, the observed data, the first data and the second data.

[0191] Figure 9 In the illustrated embodiment, 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. In this way, the correction of the estimated state quantity can be achieved without predetermining the first type of analytical solution.

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

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

[0194] 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 an inertial measurement unit;

[0195] The first inertial measurement unit collects first data and the second inertial measurement unit collects second data.

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

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

[0198] For example, when the first device is a movable device that is connected to the second device for communication, and the user uses the movable device to control the second device, the preset observation relative position may be a relatively small value between 0 and 2 meters, which is consistent with the possible distance between the two devices in this case.

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

[0200] In some alternative embodiments of the present disclosure, the target sensor may include either a vision sensor, such as an image sensor, or a non-vision sensor. For example, the target sensor may include a first magnetometer disposed on a first device and a second magnetometer disposed on a second device. Correspondingly, the observation data may include first magnetic measurement data collected by the first magnetometer and second magnetic measurement data collected by the second magnetometer. As another example, the target sensor may include one of the following two: a first image sensor disposed on the first device, a second image sensor disposed on the second device. Correspondingly, the observation data may include one of the following two: an image of the second device collected by the first image sensor, an image of the first device collected by the second device.

[0201] In an alternative example, as Figure 4-1 shown, 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 a rear passenger in the vehicle 410. An image sensor may be disposed on the surface of the portable device 415, and the observation data may include an image of the augmented reality glasses 420 collected by the image sensor disposed on the portable device 415.

[0202] In another alternative example, as Figure 4-2 shown, the first device may be the vehicle 410, and the second device may be augmented reality glasses 420 worn by a rear passenger in the vehicle. The augmented reality glasses 420 may be provided with an image sensor, and the observation data may include an image collected by the image sensor disposed on the augmented reality glasses 420.

[0203] In yet another alternative example, as Figure 4-3 shown, 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 be respectively provided with magnetometers, and the observation data may include magnetic measurement data respectively collected by the magnetometer disposed on the augmented reality glasses 430 and the magnetometer disposed on the adapter device 440.

[0204] In the embodiments of the present disclosure, at least one of a preset observation 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 may be used to correct the prior state quantity. In this way, it is possible to provide a very effective reference for correcting the prior state quantity, which is beneficial to ensuring the correction effect.

[0205] In some alternative embodiments of the present disclosure, the parameter located in the i-th row and j-th column of the discrete-time error state transition matrix may be represented as Φ ij(t, t0), the first type of analytical solution may include at least one of the following items:

[0206]

[0207]

[0208]

[0209]

[0210]

[0211] Φ 12 (t, t0) = C′(t)(t - t0)

[0212]

[0213]

[0214]

[0215]

[0216]

[0217]

[0218]

[0219]

[0220]

[0221]

[0222]

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

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

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

[0226] Wherein,

[0227] Wherein, 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 the error state transition, and t represents the target time of the error state transition. 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, and [×] represents the skew-symmetric matrix of the parameter in []. Represents the difference between the angular velocity measured by the first inertial measurement unit at time t0 and the angular velocity bias of the first inertial measurement unit at time t0. Represents the difference between the angular velocity measured by the first inertial measurement unit at time t and the angular velocity bias of the first inertial measurement unit at time t. Represents The attitude of the first inertial measurement unit at time relative to the first inertial measurement unit at time t0. Represents The attitude of the second inertial measurement unit at time relative to The first inertial measurement unit at time. Represents the acceleration measured by the second inertial measurement unit at Time and The difference between the acceleration bias of the second inertial measurement unit at time. Represents the attitude of the second inertial measurement unit at time t0 relative to The first inertial measurement unit at time. Is between t0 and s. Represents the attitude of the first inertial measurement unit at time μ relative to The first inertial measurement unit at time, where μ is between t0 and Between. Represents the attitude of the second inertial measurement unit at time μ relative to The first inertial measurement unit at time. Represents The attitude of the second inertial measurement unit at time relative to the first inertial measurement unit at time t0. 03 represents the zero matrix, and I3 represents the identity matrix.

[0228] Next, the principle of obtaining the first type of analytical solution will be introduced in detail.

[0229] As introduced above, for example, the continuous-time error state transition matrix can be represented as F, and F can be in the following form:

[0230]

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

[0232]

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

[0234] The parameter in the i-th row and j-th column of Φ(t, t0) can be expressed as Φ ij (t, t0) or Φ ij . Next, the case of i > 3 will be discussed first. In this case, the following formula can be obtained:

[0235]

[0236] Φ ij (t, t0) = Φ ij (t0, t0) = I3 or 03

[0237] Still assuming i > 3, if i = j, then there is:

[0238] Φ ij (t, t0) = Φ ij (t0, t0) = I3

[0239] Still assuming i > 3, if i ≠ j, then there is:

[0240] Φ ij (t, t0) = Φ ij (t0, t0) = 03

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

[0242]

[0243] Next, the case of i ≤ 3 will be discussed.

[0244] Optionally, Φ 31 , Φ 32 , Φ 36 , Φ 37 can be determined according to the following derivation process:

[0245]

[0246]

[0247] Φ3j (t, t0) = 03, j = 1, 2, 6, 7

[0248] Optionally, Φ can be determined according to the following derivation process 33 :

[0249]

[0250] Optionally, Φ can be determined according to the following derivation process 34 :

[0251]

[0252] Optionally, Φ can be determined according to the following derivation process 35 :

[0253]

[0254] Optionally, Φ can be determined according to the following derivation process 11 and Φ 21 :

[0255]

[0256]

[0257] Φ 11 (t0, t0) = I3

[0258] Φ 21 (t0, t0) = 03

[0259]

[0260]

[0261]

[0262] Optionally, Φ can be determined according to the following derivation process 12 and Φ 22 :

[0263]

[0264]

[0265] Φ 12 (t0) = 03

[0266] Φ 22 (t0) = I3

[0267] Φ 12 = C′(t)(t - t0)

[0268]

[0269] Optionally, Φ can be determined according to the following derivation process 13 and Φ 23 :

[0270]

[0271]

[0272] Φ 13 Φ(t0, t0) = 03

[0273] Φ 23 Φ(t0, t0) = 03

[0274]

[0275]

[0276] Optionally, Φ can be determined according to the following derivation process 14 and Φ 24 :

[0277]

[0278]

[0279] Φ 14 Φ(t0, t0) = 03

[0280] Φ 24 Φ(t0, t0) = 03

[0281]

[0282]

[0283] Optionally, Φ can be determined according to the following derivation process 15 and Φ 25 :

[0284]

[0285]

[0286] Φ 15 Φ(t0, t0) = 03

[0287] Φ 25 Φ(t0, t0) = 03

[0288]

[0289]

[0290] Optionally, Φ can be determined according to the following derivation process 16 and Φ 26 :

[0291]

[0292]

[0293] Φ 16 (t0, t0) = 03

[0294] Φ 26 (t0, t0) = 03

[0295]

[0296]

[0297] Optionally, Φ can be determined according to the following derivation process 17 and Φ 27 :

[0298]

[0299]

[0300] Φ 17 (t0, t0) = 03

[0301] Φ 27 (t0, t0) = 03

[0302]

[0303]

[0304] So far, based on the relationship that needs to be satisfied between F and Φ(t, t0), through strict mathematical derivation, the analytical solutions of each parameter in the discrete-time error state transition matrix have been obtained, and thus the first type of analytical solution can be obtained. Applying the first type of analytical solution to the determination of the discrete-time error state transition matrix is beneficial to ensuring the reliability of the determined discrete-time error state transition matrix.

[0305] It should be noted that the specific expression of F introduced above is only a possible form, and those skilled in the art can also adopt other expressions according to the actual situation. If other expressions are adopted, combined with the relationship that needs to be satisfied between F and Φ(t, t0), the corresponding analytical solutions can also be obtained through strict mathematical derivation.

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

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

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

[0309] It should be noted that the various equations included in the state estimation equation are introduced above. These equations involve various operation logics, and there are coupling relationships among many parameters involved in these equations. The integration results of some parameters in the state estimation equation may be very complex, making it difficult to derive the analytical solutions of the respective parameters in the estimated state quantity from the state estimation equation.

[0310] In view of this, in some alternative implementations of the present disclosure, before performing Figure 10 the method shown, at least one of the Runge-Kutta method and the numerical approximation method may be introduced on the basis of the state estimation equation for simplification processing to obtain the second type of analytical solution. The specific usage methods of the Runge-Kutta method and the numerical approximation method may refer to Figure 6 the relevant introductions in the embodiments shown, and will not be elaborated here. Then, the second type of analytical solution may be stored in a preset third storage area.

[0311] In step 1010, the second type of analytical solution may be obtained from the third storage area. In step 1020, only by substituting the state quantity, the first data, and the second data into the second type of analytical solution for calculation, the corresponding estimated state quantity can be obtained efficiently and reliably.

[0312] Adopting Figure 10 the embodiments shown, by introducing at least one of the Runge-Kutta method and the numerical approximation method, the second type of analytical solution can be successfully obtained. Since the constraints related to the lever arm are introduced in the state estimation equation, the second type of analytical solution obtained based on the state estimation equation can also reflect the constraints related to the lever arm. Therefore, using the second type of analytical solution for determining the estimated state quantity is beneficial to eliminating the lever arm error, thereby improving the accuracy of state estimation and further improving the relative positioning accuracy between the second device and the first device.

[0313] In some alternative embodiments of the present disclosure, a state quantity, first data, and second data can be obtained. Based on the state quantity, first data, second data, and a state estimation equation that meets the predetermined lever arm constraint condition, an estimated state quantity can be determined. In this way, by introducing lever arm-related constraints into the state estimation equation, the reliability of the estimated state quantity can be ensured.

[0314] After determining the estimated state quantity, in an example where the estimated state quantity is used as a prior state quantity, the prior state quantity can be corrected to obtain a corrected state quantity. In the correction stage, a discrete-time error state transition matrix can be determined based on a first type of analytical solution, and the discrete-time error state transition matrix can be used. In this way, the reliability of the discrete-time error state transition matrix can be ensured, which is beneficial to ensuring the reliability of the corrected state quantity. The corrected relative position and corrected relative attitude in the corrected state quantity can be used as the relative positioning result between the second device and the first device. Therefore, adopting the embodiments of the present disclosure is beneficial to improving the relative positioning accuracy between the second device and the first device.

[0315] Any method for state estimation provided by the embodiments of the present disclosure can be executed by any suitable device with data processing capabilities, including but not limited to: terminal devices, servers, etc. Alternatively, any method for state estimation provided by the embodiments of the present disclosure can be executed by a processor. For example, the processor can execute any method for state estimation mentioned in the embodiments of the present disclosure by calling the corresponding instructions stored in the memory. This will not be elaborated further below.

[0316] Exemplary Device

[0317] As Figure 11 shown, it is a schematic structural diagram of a device for state estimation provided by some exemplary embodiments of the present disclosure. Figure 11The device shown may include: a first acquisition module 1110, configured to acquire a state quantity, first data acquired by a first inertial measurement unit disposed on a first device, and second data acquired by a second inertial measurement unit disposed on a second device; wherein, the state quantity includes: 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 includes 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, and 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 1120, configured to acquire 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; a first determination module 1130, configured to determine an estimated state quantity based on the state quantity, the first data, the second data, and the state estimation equation.

[0318] In some alternative embodiments of the present disclosure, as Figure 12 shown, the device provided by the embodiment of the present disclosure may further include: a third acquisition module 1210, configured to acquire 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 transition matrix, the first type of analytical solution refers to an analytical solution of parameters in the discrete-time error state transition matrix, and the type of the target sensor is different from that of the inertial measurement unit; a second determination module 1220, configured to determine a discrete-time error state transition matrix based on the state quantity, a prior state quantity, the first data, the second data, and the first type of analytical solution; a fourth acquisition module 1230, configured to acquire a noise Jacobian matrix and a continuous-time noise covariance matrix; a third determination module 1240, configured to determine 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; a fourth determination module 1250, configured to determine 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; a first correction module 1260, configured to correct the prior state quantity based on the prior state covariance matrix and the observation data.

[0319] In some alternative embodiments of the present disclosure, as Figure 13As shown in the figure, the device provided by the embodiment of the present disclosure may further include: a fifth acquisition module 1310, configured to acquire a first type of analytical solution determined based on preset definition information, and observation data obtained by observing a first device and a second device through a target sensor; wherein, the preset definition information refers to the definition information of the parameters in the discrete-time error state transition matrix, the first type of analytical solution refers to the analytical solution of the parameters in the discrete-time error state transition matrix, and the type of the target sensor is different from that of the inertial measurement unit; a fifth determination module 1320, configured to determine a discrete-time error state transition matrix based on a state quantity, a prior state quantity, first data, second data, and the first type of analytical solution; a sixth acquisition module 1330, configured to acquire a noise Jacobian matrix and a continuous-time noise covariance matrix; a sixth determination module 1340, configured to determine 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; a seventh determination module 1350, configured to determine a 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; a second correction module 1360, configured 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.

[0320] In some alternative embodiments of the present disclosure, as Figure 14-1 、 Figure 14-2 shown, the device provided by the embodiment of the present disclosure may further include: a seventh acquisition module 1410, configured to acquire 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, the first type of analytical solution refers to the analytical solution of the parameters in the discrete-time error state transition matrix; the device provided by the embodiment of the present disclosure may satisfy one of the following: estimating the state quantity as the prior state quantity, as Figure 14-1 shown, the device further includes: a third correction module 1420, configured to correct 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; as Figure 14-2 shown, the device may further include: a fourth correction module 1430, configured to acquire the prior state quantity, and correct 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 is used as the observation data.

[0321] In some alternative embodiments of the present disclosure, as Figure 15 shown, the third correction module 1420 includes:

[0322] a first acquisition sub-module 1510, configured to acquire the state covariance matrix of the error state quantity corresponding to the state quantity and the observation data;

[0323] The first determination sub-module 1520 is configured to determine a discrete-time error state transition matrix based on a state quantity, a prior state quantity, first data, second data, and a first type of analytical solution;

[0324] The second acquisition sub-module 1530 acquires a noise Jacobian matrix and a continuous-time noise covariance matrix;

[0325] The second determination sub-module 1540 is configured to determine 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;

[0326] The third determination sub-module 1550 is configured to determine 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;

[0327] The first correction sub-module 1560 is configured to correct the prior state quantity based on the prior state covariance matrix and the observation data.

[0328] In some alternative embodiments of the present disclosure, as Figure 16 shown, the third correction module 1420 includes:

[0329] The third acquisition sub-module 1610 is configured to acquire observation data;

[0330] The fourth determination sub-module 1620 is configured to determine a discrete-time error state transition matrix based on a state quantity, a prior state quantity, first data, second data, and a first type of analytical solution;

[0331] The fourth acquisition sub-module 1630 is configured to acquire a noise Jacobian matrix and a continuous-time noise covariance matrix;

[0332] The fifth determination sub-module 1640 is configured to determine 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;

[0333] The sixth determination sub-module 1650 is configured to determine an objective 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;

[0334] The second correction sub-module 1660 is configured to correct the prior state quantity based on the discrete-time noise covariance matrix, the objective Jacobian matrix, the observation data, the first data, and the second data.

[0335] In some alternative embodiments of the present disclosure, the parameter located in the i-th row and the j-th column of the discrete-time error state transition matrix is represented as Φ ij(t, t0), then the first type of analytical solution may include at least one of the following items:

[0336]

[0337]

[0338]

[0339]

[0340]

[0341] Φ 12 (t, t0) = C′(t)(t - t0)

[0342]

[0343]

[0344]

[0345]

[0346]

[0347]

[0348]

[0349]

[0350]

[0351]

[0352]

[0353] If i = 3, and j = 1, 2, 6, 7, Φ 3j (tt0) = 03

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

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

[0356] Wherein,

[0357] In some alternative embodiments of the present disclosure, the discrete-time noise covariance matrix is denoted as Qk，k-1 , the discrete-time noise covariance matrix is calculated in the following manner:

[0358]

[0359] where, Φ(τ) represents the discrete-time error state transition matrix between the time and the (k - 1)th time, G(τ) represents the noise Jacobian matrix between the time and the (k - 1)th time, and Q c represents the continuous-time noise covariance matrix.

[0360] In some alternative embodiments of the present disclosure, as Figure 17 shown, any one of the second determination module 1220, the fifth determination module 1320, the first determination sub-module 1520, and the fourth determination sub-module 1620 may include: a fifth acquisition sub-module 1710, configured to acquire a processing result obtained by processing the first type of analytical solution according to a preset method; where the preset method includes at least one of the following: the Runge-Kutta method, the numerical approximation method; a seventh determination sub-module 1720, configured to determine the discrete-time error state transition matrix based on the state quantity, the prior state quantity, the first data, the second data, and the processing result.

[0361] In some alternative embodiments of the present disclosure, as Figure 18 shown, the device provided by the embodiment of the present disclosure further includes: an eighth acquisition module 1810, configured to acquire the continuous-time error state transition matrix and the observation data; an eighth determination module 1820, configured to determine the discrete-time error state transition matrix based on the continuous-time error state transition matrix; a ninth acquisition module 1830, configured to acquire the noise Jacobian matrix and the continuous-time noise covariance matrix; a ninth determination module 1840, configured to determine the 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; a fifth correction module 1850, configured to correct the estimated state quantity based on the discrete-time noise covariance matrix and the observation data; where 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:

[0362]

[0363]

[0364]

[0365]

[0366] In some alternative embodiments of the present disclosure, the observation data includes at least one of the following: a 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; first data collected by the first inertial measurement unit and second data collected by the second inertial measurement unit.

[0367] In some alternative embodiments of the present disclosure, the first equation defines the relationship between the derivative result of the relative velocity between the first device and the second device and a target difference, where the target difference is the difference obtained by removing the Coriolis acceleration and the transport acceleration from the reference relative acceleration. 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.

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

[0369]

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

[0371]

[0372]

[0373] 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.

[0374] In some alternative embodiments of the present disclosure, such as Figure 19As shown, the first determination module 1130 includes: a sixth acquisition sub-module 1910, configured to acquire a second type of analytical solution determined based on a 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; an eighth determination sub-module 1920, configured to determine the estimated state quantity based on the state quantity, the first data, the second data, and the second type of analytical solution.

[0375] In the device of the present disclosure, the various optional embodiments, optional implementation manners, 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.

[0376] Exemplary Electronic Device

[0377] Figure 20 The block diagram of an electronic device according to an embodiment of the present disclosure is illustrated. The electronic device 2000 includes one or more processors 2010 and a memory 2020.

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

[0379] The memory 2020 may include one or more computer program products, and the computer program products 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, etc. Non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage media, and the processor 2010 may run 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.

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

[0381] Of course, for simplicity, Figure 20Only some of the components related to the present disclosure in the electronic device 2000 are shown, and components such as buses, input / output interfaces, etc. are omitted. In addition, according to specific application scenarios, the electronic device 2000 may further include other appropriate components.

[0382] Exemplary Computer Program Product and Computer Readable Storage Medium

[0383] In addition to the above methods and devices, an embodiment of the present disclosure may also be a computer program product, which includes computer program instructions that, when run by a processor, cause the processor to execute the steps in the methods according to various embodiments of the present disclosure described in the above "Exemplary Method" section of this specification.

[0384] The computer program product can be written in any combination of one or more programming languages to write program code for performing the operations of the embodiments of the present disclosure. The programming languages include object-oriented programming languages such as Java, C++, etc., and also include conventional procedural programming languages such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computing device, partially on the user's device, executed as an independent 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.

[0385] In addition, an embodiment of the present disclosure may also be a computer-readable storage medium, on which computer program instructions are stored, and when the instructions are run by a processor, the processor is caused to execute the steps in the methods according to various embodiments of the present disclosure described in the above "Exemplary Method" section of this specification.

[0386] The computer-readable storage medium may adopt any combination of one or more readable media. The readable media may be a readable signal medium or a readable storage medium. The readable storage medium may, for example, include but is not limited to an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the above. More specific examples (non-exhaustive list) of the readable storage medium include: an electrical connection having 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 of the above.

[0387] The basic principles of the present disclosure have been described in connection with specific embodiments. However, the advantages, benefits, effects, etc. mentioned in the present disclosure are merely examples and not limitations. It cannot be considered that these advantages, benefits, effects, etc. are essential for each embodiment of the present disclosure. The specific details disclosed above are only for the purpose of illustration and easy understanding, rather than limitations. These details do not limit the present disclosure to necessarily implement with the above specific details.

[0388] Those skilled in the art can 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 equivalent technologies, the present disclosure also intends to include these changes and modifications.

Claims

1. A method for state estimation, comprising: obtaining state variables, 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 variables 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 conforms to 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 variable based on the state variables, the first data, the second data, and the state estimation equation.

2. The method according to claim 1, wherein, The method further includes: 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 includes at least one of the following: using the estimated state variable as a prior state variable, and correcting the prior state variable based on the state variables, the prior state variable, the first data, the second data, and the first type of analytical solution; obtaining a prior state variable; correcting the prior state variable based on the prior state variable, the estimated state variable, the first data, the second data, and the first type of analytical solution, wherein at least part of the estimated state variable is used as observation data.

3. The method according to claim 2, wherein The correcting the prior state variable based on the state variables, the prior state variable, the first data, the second data, and the first type of analytical solution includes: obtaining the state covariance matrix of the error state variable corresponding to the state variables and the observation data; determining the discrete-time error state transition matrix based on the state variables, the prior state variable, the first data, the second data, and the first type of analytical solution; obtaining the noise Jacobian matrix and the continuous-time noise covariance matrix; determining the 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 the 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 the 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. According to the method as claimed in 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:​ Φ 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: where, Φ(τ) 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 by the following method:

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 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, and 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. Among them, 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, 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. The method according to any one of claims 1-4 and 8, wherein, Based on the state quantity, the first data, the second data, and the state estimation equation, determining the estimated state quantity includes: Obtaining a second type of analytical solution determined based on the state estimation equation and a preset method; where the preset method includes at least one of the following: the Runge-Kutta method, the 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. A device for state estimation, comprising: A first acquisition module, configured to acquire a state quantity, first data acquired by a first inertial measurement unit disposed on a first device, and second data acquired by a second inertial measurement unit disposed on a second device; where the state quantity includes: a target parameter, 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. The target parameter includes 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. 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, configured to acquire 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 a relative velocity between the second device and the first device, and the second equation defines a relative position between the second device and the first device; A first determination module, configured to determine an estimated state quantity based on the state quantity, the first data, the second data, and the state estimation equation.

15. An electronic device, comprising: A memory, configured to store a computer program product; A processor, configured to execute the computer program product stored in the memory, and when the computer program product is executed, implement the method for state estimation according to any one of claims 1 to 13 above.

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

Images