Kalman filter methods, systems, devices, and media for multi-modal observation inputs

Abstract

The application provides a Kalman filtering method, system, device and medium for multi-mode observation input, which comprises the following steps: receiving inertial measurement unit data, adding the inertial measurement unit data and a corresponding time stamp into a first data queue; receiving sensor observation data; obtaining inertial measurement unit data meeting a time requirement from the first data queue; and using a Kalman filter to predict and update vehicle state parameters based on the inertial measurement unit data meeting the time requirement and the sensor observation data. The application uniformly carries out the prediction and update process when the sensor observation data arrives, thereby improving the reliability of vehicle state parameter update.

Description

Technical Field

[0001] This invention relates to the field of state estimation technology, and in particular to a Kalman filtering method, system, device and medium for multi-mode observation input. Background Technology

[0002] Multi-sensor fusion using Kalman-like filters is a common method in positioning technology. It allows the positioning system to maintain convergence of the fused results for a certain period even when GNSS data is lost, thanks to observations from other sensors. Common Kalman-like filters include EKF (Extended Kalman Filter), UKF (Unscented Kalman Filter), and ESKF (Error-State Kalman Filter). Common sensors used include wheel speed sensors, lidar, and cameras.

[0003] Existing Kalman filtering algorithms based on Kalman filters separate the prediction and update processes, performing them separately when IMU (Inertial Measurement Unit) data or observation sensor data arrives. This independent prediction and update process leads to highly ambiguous states in the state queue, potentially resulting in states that have only been predicted or have already been updated multiple times by different sensors. This hinders both the effective output of data with corresponding timestamps and the control of data fusion across different sensors.

[0004] Furthermore, in existing algorithms, the state variables updated by various sensors are mixed together without any distinguishing markers, making it impossible to differentiate between positioning results obtained using absolute observations and those obtained using relative observations. This design introduces its own error into the process of updating the IMU's zero-bias state using relative observations.

[0005] It should be noted that the information disclosed in the background section above is only used to enhance the understanding of the background of the present invention, and therefore may include information that does not constitute prior art known to those skilled in the art. Summary of the Invention

[0006] To address the problems in the prior art, the present invention aims to provide a Kalman filtering method, system, device, and medium for multi-mode observation input, which uniformly performs the prediction and update process when sensor observation data arrives, thereby improving the reliability of vehicle state parameter updates.

[0007] This invention provides a Kalman filtering method for multi-mode observation input, comprising the following steps:

[0008] Upon receiving inertial measurement unit (IMU) data, the IMU data and the corresponding timestamp are added to the first data queue.

[0009] Received sensor observation data;

[0010] Obtain inertial measurement unit data that meets the time requirements from the first data queue;

[0011] A Kalman-like filter is used to predict and update vehicle state parameters based on the time-compliant inertial measurement unit data and the sensor observation data.

[0012] In some embodiments, after receiving the sensor observation data, the method further includes the following steps:

[0013] Determine the type of data observed by the sensor;

[0014] If the sensor observation data is absolute observation data, add the absolute observation data and the current timestamp to the second data queue;

[0015] If the sensor observation data is relative observation data, add the relative observation data and the current timestamp to the third data queue.

[0016] In some embodiments, when the sensor observation data is absolute observation data, after adding the absolute observation data and the current timestamp to the second data queue, inertial measurement unit data that meets the first time requirement is obtained from the first data queue, and the vehicle state parameters are predicted and updated using the first processing algorithm.

[0017] When the sensor observation data is relative observation data, after adding the relative observation data and the current timestamp to the third data queue, the inertial measurement unit data that meets the second time requirement is obtained from the first data queue, and the vehicle state parameters are predicted and updated using the second processing algorithm.

[0018] In some embodiments, obtaining inertial measurement unit data that meets a first time requirement from the first data queue includes:

[0019] In the first data queue, inertial measurement unit data with timestamps between the previous update timestamp and the current timestamp are selected.

[0020] In some embodiments, the step of predicting and updating vehicle state parameters using the first processing algorithm includes the following steps:

[0021] Predict the vehicle state parameters at the current timestamp based on the inertial measurement unit data that meets the first time requirement;

[0022] Based on the vehicle state parameters at the current timestamp and the absolute observation data at the current timestamp, the updated value of the state parameters at the current timestamp is obtained.

[0023] In some embodiments, obtaining inertial measurement unit data that meets the second time requirement from the first data queue includes the following steps:

[0024] In the first data queue, inertial measurement unit (IMU) data with timestamps between the previous update timestamp and the previous acquisition timestamp of relative observation data is extracted, and IMU data within the latest two frames of relative observation time period is also extracted.

[0025] In some embodiments, the step of predicting and updating vehicle state parameters using the second processing algorithm includes the following steps:

[0026] The first vehicle state parameters are predicted based on the inertial measurement unit data whose timestamps are between the previous update timestamp and the previous acquisition of relative observation data timestamps.

[0027] The relative motion data is predicted based on the inertial measurement unit data within the latest two frames of relative observation time period, and the relative motion prediction value is obtained.

[0028] The updated value of the state parameter at the current timestamp is obtained based on the first vehicle state parameter, the relative motion prediction value, the previously acquired relative observation data, and the relative observation data at the current timestamp.

[0029] In some embodiments, obtaining the updated state parameter value at the current timestamp based on the first vehicle state parameter, the relative motion prediction value, the previously acquired relative observation data, and the relative observation data at the current timestamp includes the following steps:

[0030] The relative motion update value is calculated based on the relative motion prediction value, the previously acquired relative observation data, and the relative observation data at the current timestamp;

[0031] Based on the first vehicle state parameters and the relative motion update value, the state parameter update value of the current timestamp is obtained.

[0032] In some embodiments, after the vehicle state parameters are predicted and updated using the first processing algorithm, the updated state parameter values ​​at the current timestamp are added with an absolute observation source identifier and then added to the fourth data queue.

[0033] After the second processing algorithm is used to predict and update the vehicle state parameters, the updated state parameter values ​​with the current timestamp are added with the relative observation source identifier and then added to the fourth data queue.

[0034] In some embodiments, after receiving sensor observation data, when the sensor observation data is absolute observation data, before adding the absolute observation data and the current timestamp to the second data queue, the following steps are further included:

[0035] Determine whether the Kalman filter has been initialized;

[0036] If so, add the absolute observation data and the current timestamp to the second data queue;

[0037] If not, the Kalman filter is initialized using the absolute observation data, and then the current processing flow ends.

[0038] In some embodiments, after receiving sensor observation data, when the sensor observation data is relative observation data, before adding the relative observation data and the current timestamp to the third data queue, the following steps are further included:

[0039] Determine whether the Kalman filter has been initialized;

[0040] If so, add the relative observation data and the current timestamp to the third data queue;

[0041] If not, then end the current processing flow.

[0042] In some embodiments, the following steps are also included:

[0043] In the fourth data queue, find the number of consecutive occurrences of the state parameter update value with the absolute observation source identifier that is closest to the current time;

[0044] Determine whether the number of consecutive occurrences is greater than or equal to a preset threshold.

[0045] If so, the zero bias of the inertial measurement unit is updated based on the most recent absolute observation data.

[0046] This invention also provides a Kalman filter system for multi-mode observation inputs, used to implement the aforementioned Kalman filter method for multi-mode observation inputs. The system includes:

[0047] The first data receiving module is used to receive inertial measurement unit data and add the inertial measurement unit data and the corresponding timestamp to the first data queue.

[0048] The second data receiving module is used to receive sensor observation data;

[0049] The data acquisition module is used to acquire inertial measurement unit data that meets the time requirements from the first data queue;

[0050] The parameter update module is used to predict and update vehicle state parameters based on the time-compliant inertial measurement unit data and the sensor observation data, using a Kalman-like filter.

[0051] This invention also provides a Kalman filter device for multi-mode observation input, comprising:

[0052] processor;

[0053] A memory in which executable instructions of the processor are stored;

[0054] The processor is configured to perform the steps of the Kalman filtering method for the multi-mode observation input by executing the executable instructions.

[0055] This invention also provides a computer-readable storage medium for storing a program that, when executed by a processor, implements the steps of the Kalman filtering method for the multi-mode observation input.

[0056] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and are not intended to limit this disclosure.

[0057] The Kalman filtering method, system, device, and medium for multi-mode observation input of the present invention have the following beneficial effects:

[0058] By employing this invention, when inertial measurement unit (IMU) data is received, it is timestamped and added to the first data queue. After receiving sensor observation data, the corresponding IMU data is retrieved from the first data queue. Then, vehicle state parameters are updated and predicted based on a Kalman-like filter, thereby achieving a unified prediction and update process and improving the reliability of vehicle state parameter updates. Attached Figure Description

[0059] Other features, objects, and advantages of the invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings.

[0060] Figure 1 This is a flowchart of a Kalman filtering method for multi-mode observation input according to an embodiment of the present invention;

[0061] Figure 2 This is a flowchart illustrating the processing of absolute observation data and relative observation data according to an embodiment of the present invention;

[0062] Figure 3 This is a flowchart of a first processing algorithm used to predict and update vehicle state parameters according to an embodiment of the present invention;

[0063] Figure 4This is a flowchart of a method for predicting and updating vehicle state parameters using a second processing algorithm according to an embodiment of the present invention;

[0064] Figure 5 This is a comparison diagram showing the effects of the method of this invention and the methods of the prior art;

[0065] Figure 6 This is a schematic diagram of the structure of a Kalman filter system with multi-mode observation input according to an embodiment of the present invention;

[0066] Figure 7 This is a schematic diagram of the structure of a Kalman filter device for multi-mode observation input according to an embodiment of the present invention;

[0067] Figure 8 This is a schematic diagram of the structure of a computer-readable storage medium according to an embodiment of the present invention. Detailed Implementation

[0068] Exemplary embodiments will now be described more fully with reference to the accompanying drawings. However, these exemplary embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, they are provided so that this disclosure will be more comprehensive and complete, and will fully convey the concept of the exemplary embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.

[0069] Furthermore, the accompanying drawings are merely illustrative of this disclosure and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and therefore repeated descriptions of them will be omitted. Some block diagrams shown in the drawings are functional entities and do not necessarily correspond to physically or logically independent entities. These functional entities may be implemented in software, in one or more hardware modules or integrated circuits, or in different network and / or processor devices and / or microcontroller devices.

[0070] The flowchart shown in the attached diagram is merely an illustrative example and does not necessarily include all steps. For example, some steps may be broken down, while others may be combined or partially combined. Therefore, the actual execution order may change depending on the specific circumstances.

[0071] like Figure 1 As shown, this embodiment of the invention provides a Kalman filtering method for multi-mode observation input, comprising the following steps:

[0072] S100: Upon receiving data from the inertial measurement unit, the inertial measurement unit data and the corresponding timestamp are added to a first data queue; the first data queue is used to store the measurement data of the inertial measurement unit.

[0073] S200: Received sensor observation data;

[0074] In this embodiment, the sensor observation data can be absolute observation data or relative observation data. Absolute observation data refers to GNSS data, while relative observation data refers to data obtained by sensors such as lidar, wheel speedometer, and camera.

[0075] When GNSS data is available, it can be used to accurately locate vehicles. In the event of GNSS data loss, the convergence of the fused positioning results can be maintained for a certain period of time through data processing of relative observation data.

[0076] S300: Obtain inertial measurement unit data that meets the time requirements from the first data queue; this inertial measurement unit data that meets the time requirements is the data corresponding to the time of the sensor observation data just received;

[0077] S400: Using a Kalman-like filter, vehicle state parameters are predicted and updated based on the time-compliant inertial measurement unit data and the sensor observation data. After obtaining the updated state parameter values ​​at the current timestamp through step S400, these values ​​can be output to the fourth data queue to complete the update of the state parameter data estimation results. After outputting the updated state parameter values ​​to the fourth data queue, the current loop is completed, and the next loop can continue after a certain period of time.

[0078] In this embodiment, a Kalman-like filter can be used, such as EKF, UKF, or ESKF. The vehicle state parameters can include the vehicle's position, vehicle speed, vehicle direction, the bias value of the accelerometer in the inertial measurement unit (IMU), and the bias value of the gyroscope in the IMU for vehicle positioning. For example, the final updated state parameters are in the form of a state equation: x = [pv θ b] f b ω ] T Where p is the vehicle position, v is the vehicle speed, θ is the vehicle direction, and b f b is the bias value of the accelerometer in the inertial measurement unit. w Here, T represents the bias value of the gyroscope in the inertial measurement unit, and T denotes matrix transpose.

[0079] This invention employs step S100, where, upon receiving inertial measurement unit (IMU) data, a timestamp is added and the data is added to the first data queue. Steps S200 and S300, upon receiving sensor observation data, the corresponding IMU data is retrieved from the first data queue. Then, in step S400, vehicle state parameters are updated and predicted based on a Kalman-like filter. This achieves a unified prediction and update process upon the arrival of sensor observation data, improving the reliability of Kalman filtering for multi-mode observation inputs. Furthermore, it reduces positioning errors using relative observation data when absolute observation data is lost, improving the positioning performance of integrated navigation in scenarios such as tunnels and near tall buildings.

[0080] like Figure 2 As shown, in this embodiment, after receiving the sensor observation data, step S200 further includes the following steps:

[0081] S210: Determine the type of data observed by the sensor;

[0082] If the sensor observation data is absolute observation data, S220: Add the absolute observation data and the current timestamp to the second data queue;

[0083] If the sensor observation data is relative observation data, S230: add the relative observation data and the current timestamp to the third data queue.

[0084] like Figure 2 As shown, in this embodiment, when the sensor observation data is absolute observation data, after step S220: adding the absolute observation data and the current timestamp to the second data queue, step S310: obtaining inertial measurement unit data that meets the first time requirement from the first data queue, and step S410: using the first processing algorithm to predict and update the vehicle state parameters;

[0085] When the sensor observation data is relative observation data, after step S230: adding the relative observation data and the current timestamp to the third data queue, step S320: obtaining inertial measurement unit data that meets the second time requirement from the first data queue, and step S420: using the second processing algorithm to predict and update the vehicle state parameters.

[0086] Therefore, this invention uses different processing algorithms to predict and update vehicle state parameters when absolute observation data arrives and when relative observation data arrives, that is, it processes absolute observation and relative observation separately, thereby distinguishing between vehicle state parameters obtained from absolute observation and vehicle state parameters obtained from relative observation.

[0087] In this embodiment, after receiving sensor observation data, when the sensor observation data is absolute observation data, before step S220: adding the absolute observation data and the current timestamp to the second data queue, the following steps are further included:

[0088] Determine whether the Kalman filter has been initialized;

[0089] If so, continue with steps S221, S310, and S410, that is, update the vehicle state parameters based on the received absolute observation data.

[0090] If not, the Kalman filter is initialized using the absolute observation data, and then the current processing flow ends.

[0091] In this embodiment, step S310: obtaining inertial measurement unit data that meets the first time requirement from the first data queue includes:

[0092] In the first data queue, inertial measurement unit data with timestamps between the previous update timestamp and the current timestamp are selected. The previous update timestamp refers to the timestamp of the vehicle state parameters update in the previous loop.

[0093] like Figure 3 As shown, in this embodiment, step S410: predicting and updating vehicle state parameters using a first processing algorithm includes the following steps:

[0094] S411: Predict the vehicle state parameters at the current timestamp based on the inertial measurement unit data that meets the first time requirement;

[0095] Specifically, a vehicle prediction equation can be constructed based on the inertial measurement unit data that meets the first time requirement, and the predicted vehicle state parameters at the current timestamp can be calculated. The vehicle prediction equation is as follows:

[0096]

[0097] in, Let δx be the predicted value at the current time. The state equation at the previous moment, where u is the system noise.

[0098]

[0099]

[0100] This is the way the navigation angular velocity output by the inertial measurement unit is expressed in the Earth coordinate system. This refers to the way the navigation angular velocity output by the inertial measurement unit is expressed relative to the Earth coordinate system in the inertial coordinate system. f represents the way the navigation angular velocity output by the inertial measurement unit is expressed in the inertial coordinate system. b The acceleration output by the inertial measurement unit. I is the transformation matrix from the navigation coordinate system to the vehicle coordinate system. 3*3 It is a 3x3 identity matrix, 0 3*3 0 6*3 The matrices F(x) are 3x3 and 6x3 matrices with values ​​of 0, respectively. The single × in F(x) denotes the antisymmetric matrix of the vectors.

[0101] S412: Based on the predicted vehicle state parameters at the current timestamp and the absolute observation data at the current timestamp, obtain the updated state parameter values ​​at the current timestamp;

[0102] Specifically, based on the predicted vehicle state parameters at the current timestamp and the absolute observation data at the current timestamp, a vehicle update equation can be constructed to calculate the updated state parameter values ​​at the current timestamp. The vehicle update equation is as follows:

[0103]

[0104]

[0105]

[0106] Where K is the Kalman gain, T represents the matrix transpose, and P k Let the state covariance be at the current moment. For P k The state covariance of the previous time step, where R is the observation noise. x is the predicted value at the current time calculated according to the prediction equation. k Let z be the state equation to be calculated at the current time, z be the absolute observation data at the current timestamp, and H be the observation matrix.

[0107] In this embodiment, after receiving sensor observation data, when the sensor observation data is relative observation data, before step S230: adding the relative observation data and the current timestamp to the third data queue, the following steps are further included:

[0108] Determine whether the Kalman filter has been initialized;

[0109] If so, add the relative observation data and the current timestamp to the third data queue;

[0110] If not, then end the current processing flow.

[0111] Therefore, vehicle state parameters can only be updated based on relative observation data after the Kalman filter has been initialized with absolute observation data.

[0112] In this embodiment, step S320 specifically includes:

[0113] In the first data queue, inertial measurement unit (IMU) data with timestamps between the previous update timestamp and the previous relative observation data acquisition timestamp are extracted, and IMU data within the latest two frames of relative observation time period are also extracted. The latest two frames of relative observation time period refer to the time period of the previous relative observation data acquisition and the current relative observation data acquisition time period.

[0114] like Figure 4 As shown, in this embodiment, step S420: predicting and updating vehicle state parameters using a second processing algorithm includes the following steps:

[0115] S421: Based on the inertial measurement unit data with timestamps between the previous update timestamp and the previous acquisition of relative observation data timestamp, predict the first vehicle state parameter. The first vehicle state parameter predicted here is the predicted vehicle state parameter at the previous acquisition of relative observation data timestamp. Similarly, the prediction equation described above can also be used to predict the first vehicle state parameter here, replacing the inertial measurement unit data in step S411 with the inertial measurement unit data with timestamps between the previous update timestamp and the previous acquisition of relative observation data timestamp.

[0116] S422: Based on the inertial measurement unit data within the latest two frames of relative observation time period, predict the relative motion data and obtain the relative motion prediction value;

[0117] For example, the prediction equation described above can also be used to predict the relative motion values ​​here, replacing the inertial measurement unit data in step S411 with the inertial measurement unit data within the latest two frames of relative observation time period.

[0118] S423: Based on the first vehicle state parameters, the relative motion prediction value, the previously acquired relative observation data, and the relative observation data of the current timestamp, obtain the updated state parameter value for the current timestamp.

[0119] In this embodiment, step S423, obtaining the updated state parameter value at the current timestamp based on the first vehicle state parameter, the relative motion prediction value, the previously acquired relative observation data, and the relative observation data at the current timestamp, includes the following steps:

[0120] The relative motion update value is calculated based on the relative motion prediction value, the previously acquired relative observation data, and the relative observation data at the current timestamp;

[0121] For example, the relative motion value obtained from the relative observation data is calculated by combining the previously acquired relative observation data with the relative observation data at the current timestamp. Then, based on the relative motion prediction value and the relative motion value obtained from the relative observation data, the above update equation is used to replace z with the relative motion value obtained from the relative observation data, and the relative motion update value is calculated.

[0122] Based on the first vehicle state parameter and the relative motion update value, the state parameter update value for the current timestamp is obtained. Specifically, the first vehicle state parameter and the relative motion update value are superimposed to obtain the state parameter update value for the current timestamp.

[0123] In this embodiment, step S410 involves using the first processing algorithm to predict and update the vehicle state parameters, then adding the absolute observation source identifier to the obtained current timestamp state parameter update value, and finally adding it to the fourth data queue.

[0124] Step S420: After predicting and updating the vehicle state parameters using the second processing algorithm, the updated state parameter values ​​at the current timestamp are added with the relative observation source identifier and then added to the fourth data queue.

[0125] Therefore, by processing absolute and relative observation data separately, this invention can effectively distinguish the updated state parameter values ​​obtained from different observation sources and add different identifiers to them. Thus, the zero bias of the inertial measurement unit (IMU) is only updated when the most recent data in the current fourth data queue have been updated by absolute observation data. Here, the zero bias of the IMU is, for example, the bias value of the IMU's accelerometer and the bias value of its gyroscope in the aforementioned state equation. If this zero bias is not updated, the bias value in the state equation uses the bias value from the previous update timestamp. This effectively avoids introducing errors from the relative observation data itself when updating the zero bias of the IMU using relative observation data. Specifically, the Kalman filtering method for multi-mode observation input further includes the following steps:

[0126] In the fourth data queue, find the number of consecutive occurrences of the state parameter update value with the absolute observation source identifier that is closest to the current time;

[0127] Determine whether the number of consecutive occurrences is greater than or equal to a preset threshold; this preset threshold is a pre-set empirical value, such as 10 times, 20 times, etc., and can be adjusted as needed.

[0128] If so, update the zero bias value of the inertial measurement unit based on the most recent absolute observation data;

[0129] If not, the current loop does not update the zero bias value of the inertial measurement unit.

[0130] Therefore, this invention effectively distinguishes between results obtained from absolute and relative observation data by processing sensor data obtained from absolute and relative observation data separately and assigning different labels to the vehicle state parameter results. Furthermore, by more accurately estimating the zero-bias state of the inertial measurement unit, the fusion positioning performance is further improved when absolute observation data is lost.

[0131] like Figure 5 The figure shows a comparison of the effects of updating vehicle state parameters using the method of this invention and existing methods. The coordinate unit is meters. In the figure, A represents the vehicle trajectory prediction curve using GNSS prediction throughout the entire process; B represents the vehicle trajectory prediction curve using the method of this invention after GNSS loss at point M; and C represents the vehicle trajectory prediction curve using the existing method after GNSS loss at point M. Figure 5 As can be seen, using the method of the present invention, a large lateral error only occurs after the vehicle has made five right-angle turns, and good fusion positioning results can still be maintained even after a long period of GNSS loss. The lateral and longitudinal positioning errors within 1 kilometer are both less than 0.5% (m / m).

[0132] like Figure 6 As shown, this embodiment of the invention also provides a Kalman filter system for multi-mode observation input, used to implement the aforementioned Kalman filter method for multi-mode observation input. The system includes:

[0133] The first data receiving module M100 is used to receive inertial measurement unit data and add the inertial measurement unit data and the corresponding timestamp to the first data queue.

[0134] The second data receiving module M200 is used to receive sensor observation data;

[0135] The data acquisition module M300 is used to acquire inertial measurement unit data that meets the time requirements from the first data queue;

[0136] The parameter update module M400 is used to predict and update vehicle state parameters based on the time-compliant inertial measurement unit data and the sensor observation data using a Kalman-like filter.

[0137] In the Kalman filter system with multi-mode observation input of the present invention, the functions of each module can be implemented by the specific implementation of the Kalman filter method with multi-mode observation input as described above, which will not be elaborated here.

[0138] This invention employs a first data receiving module M100, which, upon receiving inertial measurement unit (IMU) data, adds a timestamp and adds it to a first data queue. Then, through a data acquisition module M300, after receiving sensor observation data, the corresponding IMU data is retrieved from the first data queue. Finally, a parameter update module M400 updates and predicts vehicle state parameters based on a Kalman-like filter, thus achieving a unified prediction and update process and improving the reliability of vehicle state parameter updates. Furthermore, it reduces positioning errors when using relative observation data in cases of absolute observation data loss, improving the positioning performance of the integrated navigation system in scenarios such as tunnels and near tall buildings.

[0139] This invention also provides a Kalman filter device for multi-mode observation input, including a processor; a memory storing executable instructions of the processor; wherein the processor is configured to perform the steps of the Kalman filter method for multi-mode observation input by executing the executable instructions.

[0140] Those skilled in the art will understand that various aspects of the present invention can be implemented as systems, methods, or program products. Therefore, various aspects of the present invention can be specifically implemented in the following forms: a completely hardware implementation, a completely software implementation (including firmware, microcode, etc.), or a combination of hardware and software aspects, collectively referred to herein as a "circuit," "module," or "platform."

[0141] The following reference Figure 7 To describe an electronic device 600 according to this embodiment of the present invention. Figure 7 The electronic device 600 shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of the present invention.

[0142] like Figure 7 As shown, the electronic device 600 is presented in the form of a general-purpose computing device. The components of the electronic device 600 may include, but are not limited to: at least one processing unit 610, at least one storage unit 620, a bus 630 connecting different system components (including storage unit 620 and processing unit 610), a display unit 640, etc.

[0143] The storage unit stores program code that can be executed by the processing unit 610, causing the processing unit 610 to perform the steps described in the above-described section on the Kalman filtering method for multi-mode observation inputs according to various exemplary embodiments of the present invention. For example, the processing unit 610 can perform, as follows: Figure 1 The steps are shown in the figure.

[0144] The storage unit 620 may include a readable medium in the form of a volatile storage unit, such as a random access memory unit (RAM) 6201 and / or a cache storage unit 6202, and may further include a read-only memory unit (ROM) 6203.

[0145] The storage unit 620 may also include a program / utility 6204 having a set (at least one) program module 6205, such program module 6205 including but not limited to: an operating system, one or more application programs, other program modules and program data, each or some combination of these examples may include an implementation of a network environment.

[0146] Bus 630 can represent one or more of several types of bus structures, including a memory cell bus or memory cell controller, a peripheral bus, a graphics acceleration port, a processing unit, or a local bus using any of the various bus structures.

[0147] Electronic device 600 can also communicate with one or more external devices 700 (e.g., keyboard, pointing device, Bluetooth device, etc.), and with one or more devices that enable a user to interact with electronic device 600, and / or with any device that enables electronic device 600 to communicate with one or more other computing devices (e.g., router, modem, etc.). This communication can be performed via input / output (I / O) interface 650. Furthermore, electronic device 600 can also communicate with one or more networks (e.g., local area network (LAN), wide area network (WAN), and / or public networks, such as the Internet) via network adapter 660. Network adapter 660 can communicate with other modules of electronic device 600 via bus 630. It should be understood that, although not shown in the figures, other hardware and / or software modules can be used in conjunction with electronic device 600, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems.

[0148] In the Kalman filter device for multi-mode observation input, when the program in the memory is executed by the processor, it implements the steps of the Kalman filter method for multi-mode observation input. Therefore, the device can also achieve the technical effects of the aforementioned Kalman filter for multi-mode observation input.

[0149] This invention also provides a computer-readable storage medium for storing a program that, when executed by a processor, implements the steps of the Kalman filtering method for multi-mode observation inputs. In some possible embodiments, various aspects of the invention can also be implemented as a program product comprising program code that, when executed on a terminal device, causes the terminal device to perform the steps described in the above-described section on the Kalman filtering method for multi-mode observation inputs according to various exemplary embodiments of the invention.

[0150] refer to Figure 8 As shown, a program product 800 for implementing the above-described method according to an embodiment of the present invention is described. It may employ a portable compact disc read-only memory (CD-ROM) and include program code, and may be executed on a terminal device, such as a personal computer. However, the program product of the present invention is not limited thereto. In this document, the readable storage medium may be any tangible medium containing or storing a program that may be used by or in conjunction with an instruction execution system, apparatus, or device.

[0151] The program product may employ any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: an electrical connection having one or more wires, a portable disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0152] The computer-readable storage medium may include data signals propagated in baseband or as part of a carrier wave, carrying readable program code. Such propagated data signals may take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. The readable storage medium may also be any readable medium other than a readable storage medium, capable of transmitting, propagating, or transmitting programs for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the readable storage medium may be transmitted using any suitable medium, including but not limited to wireless, wired, optical fiber, RF, etc., or any suitable combination thereof.

[0153] Program code for performing the operations of this invention can be written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Java and C++, and conventional procedural programming languages ​​such as C or similar languages. The program code can execute entirely on the user's computing device, partially on the user's device, as a standalone 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. In cases involving remote computing devices, the remote computing device can be connected to the user's computing device via any type of network, including a local area network (LAN) or a wide area network (WAN), or it can be connected to an external computing device (e.g., via the Internet using an Internet service provider).

[0154] When the program in the computer storage medium is executed by the processor, it implements the steps of the Kalman filtering method for multi-mode observation input. Therefore, the computer storage medium can also obtain the technical effects of the Kalman filtering method for multi-mode observation input.

[0155] The above description, in conjunction with specific preferred embodiments, provides a further detailed explanation of the present invention. It should not be construed that the specific implementation of the present invention is limited to these descriptions. For those skilled in the art, various simple deductions or substitutions can be made without departing from the concept of the present invention, and all such modifications and substitutions should be considered within the scope of protection of the present invention.

Claims

1. A Kalman filtering method for multi-mode observation input, characterized in that, Includes the following steps: Upon receiving inertial measurement unit (IMU) data, the IMU data and the corresponding timestamp are added to the first data queue. Receive sensor observation data; determine the type of the sensor observation data; if the sensor observation data is absolute observation data, add the absolute observation data and the current timestamp to the second data queue; If the sensor observation data is relative observation data, add the relative observation data and the current timestamp to the third data queue; Inertial measurement unit (IMU) data that meets the time requirements is obtained from the first data queue; wherein, when the sensor observation data is absolute observation data, IMU data that meets the first time requirements is obtained from the first data queue; when the sensor observation data is relative observation data, IMU data that meets the second time requirements is obtained from the first data queue. A Kalman-like filter is used to predict and update vehicle state parameters based on the time-compliant inertial measurement unit data and the sensor observation data. When the sensor observation data is absolute observation data, a first processing algorithm is used to predict and update the vehicle state parameters; when the sensor observation data is relative observation data, a second processing algorithm is used to predict and update the vehicle state parameters.

2. The Kalman filtering method for multi-mode observation input according to claim 1, characterized in that, Obtain inertial measurement unit data that meets the first time requirement from the first data queue, including: In the first data queue, inertial measurement unit data with timestamps between the previous update timestamp and the current timestamp are selected.

3. The Kalman filtering method for multi-mode observation input according to claim 2, characterized in that, The process of predicting and updating vehicle state parameters using the first processing algorithm includes the following steps: Predict the vehicle state parameters at the current timestamp based on the inertial measurement unit data that meets the first time requirement; Based on the predicted vehicle state parameters at the current timestamp and the absolute observation data at the current timestamp, the updated state parameter values ​​at the current timestamp are obtained.

4. The Kalman filtering method for multi-mode observation input according to claim 1, characterized in that, Obtaining inertial measurement unit data that meets the second time requirement from the first data queue includes the following steps: In the first data queue, inertial measurement unit (IMU) data with timestamps between the previous update timestamp and the previous acquisition timestamp of relative observation data is extracted, and IMU data within the latest two frames of relative observation time period is also extracted.

5. The Kalman filtering method for multi-mode observation input according to claim 4, characterized in that, The process of predicting and updating vehicle state parameters using the second processing algorithm includes the following steps: The first vehicle state parameters are predicted based on the inertial measurement unit data whose timestamps are between the previous update timestamp and the previous acquisition of relative observation data timestamps. The relative motion data is predicted based on the inertial measurement unit data within the latest two frames of relative observation time period, and the relative motion prediction value is obtained. The updated value of the state parameter at the current timestamp is obtained based on the first vehicle state parameter, the relative motion prediction value, the previously acquired relative observation data, and the relative observation data at the current timestamp.

6. The Kalman filtering method for multi-mode observation input according to claim 5, characterized in that, The updated state parameter value for the current timestamp is obtained based on the first vehicle state parameter, the relative motion prediction value, the previously acquired relative observation data, and the relative observation data at the current timestamp, including the following steps: The relative motion update value is calculated based on the relative motion prediction value, the previously acquired relative observation data, and the relative observation data at the current timestamp; Based on the first vehicle state parameters and the relative motion update value, the state parameter update value of the current timestamp is obtained.

7. The Kalman filtering method for multi-mode observation input according to claim 2 or 6, characterized in that, After the first processing algorithm is used to predict and update the vehicle state parameters, the updated state parameter values ​​at the current timestamp are added with an absolute observation source identifier and then added to the fourth data queue. After the second processing algorithm is used to predict and update the vehicle state parameters, the updated state parameter values ​​with the current timestamp are added with the relative observation source identifier and then added to the fourth data queue.

8. The Kalman filtering method for multi-mode observation input according to claim 1, characterized in that, After receiving sensor observation data, if the sensor observation data is absolute observation data, before adding the absolute observation data and the current timestamp to the second data queue, the following steps are also included: Determine whether the Kalman filter has been initialized; If so, add the absolute observation data and the current timestamp to the second data queue; If not, the Kalman-like filter is initialized using the absolute observation data, and then the current processing flow ends.

9. The Kalman filtering method for multi-mode observation input according to claim 8, characterized in that, After receiving sensor observation data, when the sensor observation data is relative observation data, before adding the relative observation data and the current timestamp to the third data queue, the following steps are also included: Determine whether the Kalman filter has been initialized; If so, add the relative observation data and the current timestamp to the third data queue; If not, then end the current processing flow.

10. The Kalman filtering method for multi-mode observation input according to claim 7, characterized in that, It also includes the following steps: In the fourth data queue, find the number of consecutive occurrences of the state parameter update value with the absolute observation source identifier that is closest to the current time; Determine whether the number of consecutive occurrences is greater than or equal to a preset threshold. If so, the zero bias of the inertial measurement unit is updated based on the most recent absolute observation data.

11. A Kalman filter system for multi-mode observation input, characterized in that, The system for implementing the Kalman filtering method for multi-mode observation inputs according to any one of claims 1 to 10, the system comprising: The first data receiving module is used to receive inertial measurement unit data and add the inertial measurement unit data and the corresponding timestamp to the first data queue. The second data receiving module is used to receive sensor observation data; The data acquisition module is used to acquire inertial measurement unit data that meets the time requirements from the first data queue; The parameter update module is used to predict and update vehicle state parameters based on the time-compliant inertial measurement unit data and the sensor observation data, using a Kalman-like filter.

12. A Kalman filter device for multi-mode observation input, characterized in that, include: processor; A memory in which executable instructions of the processor are stored; The processor is configured to perform the steps of the Kalman filtering method for the multi-mode observation input as described in any one of claims 1 to 10 by executing the executable instructions.

13. A computer-readable storage medium for storing a program, characterized in that, When the program is executed by the processor, it implements the steps of the Kalman filtering method for multi-mode observation inputs as described in any one of claims 1 to 10.

Images