Method for adaptive compensation of vehicle onboard sensors motion with dynamic tuning of kalman filter based on computer vision confidence metrics and vehicle system implementing method

Abstract

FIELD: automotive engineering.SUBSTANCE: invention relates to systems for compensating the movement of on-board sensors of a vehicle. An extended Kalman filter state vector is formed to compensate for optical sensor delays, and the measurement noise covariance matrices are dynamically scaled based on the semantic segmentation metric of images and online adaptation of the parameters of the virtual motion model.EFFECT: increase in the accuracy of road surface profile assessment.8 cl, 9 dwg

Description

[0001] The invention relates to automotive engineering, specifically to active safety systems, adaptive cruise control systems, autonomous driving systems, and intelligent sensor data processing. More specifically, the invention relates to methods for filtering and merging inertial and optical sensor data to compensate for vibrations, roll, pitch, and vertical movements of a vehicle body when constructing a road surface profile.

[0002] A method for compensating for the movement of on-board vehicle sensors is known (US Patent No. US 10235817, B2, published 03 / 19 / 2019) “Motion Compensation For On-Board Vehicle Sensors” [1].

[0003] The specified method includes:

[0004] - receiving signals from multiple on-board sensors (cameras, lidars, IMU, gyroscopes, accelerometers);

[0005] - vehicle motion estimation using virtual motion model and Kalman filter;

[0006] - correction of front sensor data by applying matrix coordinate transformations to compensate for body disturbances;

[0007] - Iteratively refine the motion estimation based on the adjusted road profile data.

[0008] The mathematical basis of the prototype is based on three coordinate systems:

[0009] 1. Global inertial coordinate system ;

[0010] 2. "Unperturbed" body coordinate system (plane parallel to the estimated plane of the road);

[0011] 3. "Perturbed" body coordinate system (actual position with roll, pitch, yaw, vertical movement).

[0012] Compensation is performed by applying the inverse transformation matrix:

[0013]

[0014] where – vector of corrected sensor data, – correction matrix, – vector of noisy (disturbed) sensor data, is based on the assessment of the motion parameters ( - list, - pitch, - yaw, – vertical movement).

[0015] The Kalman filter [2; 3] is mentioned in the prototype as a means of correcting the predictions of the virtual model (column 14, lines 28–29; formula items 6, 16), but without detailing: adaptation of the covariance matrices of the process noise and measurements ; compensation for time delays between high-frequency inertial sensors and low-frequency optical sensors; a mathematical relationship between the filtering error and online calibration of model parameters. However, this prototype has the following drawbacks:

[0016] - Staticity of covariance matrices. In the prototype, the matrix does not adapt to changing visibility conditions (lighting, weather, road texture), resulting in suboptimal weighting of computer vision and IMU data while degrading image quality.

[0017] - External Lag Compensation. Compensation for the time lag between the IMU (frequency 100-1000 Hz) and the camera (frequency 15-30 Hz + image processing time 30-100 ms) is implemented by storing predictions in memory (Fig. 9, block 78 of the prototype), which increases the computational load and can lead to desynchronization during abrupt maneuvers.

[0018] - Unspecified "self-learning." The mention of "self-learning algorithms" for adapting model parameters (column 14, lines 20-25) does not contain a mathematical mechanism for the connection between the filtering error and the adjustment of the suspension stiffness / damping coefficients.

[0019] - Lack of connection with neural network confidence metrics. The prototype does not use the output probability maps of modern semantic segmentation algorithms for dynamically managing the confidence in optical measurements.

[0020] The technical result of the invention is to increase the accuracy and robustness of the assessment of the road surface profile and the position of the vehicle due to: a reduction in the influence of noise from optical sensors when visibility conditions deteriorate; mathematically rigorous compensation for time delays in data processing within the recursive cycle of the Kalman filter; online calibration of the parameters of the virtual motion model based on the statistics of filtering residuals.

[0021] The technical result achieved is achieved due to the fact that in the method of compensating the movement of on-board sensors of a vehicle, which includes receiving first data from inertial sensors (IMU, gyroscopes, accelerometers) and second data from optical sensors (cameras, lidars), estimating the state vector of the vehicle's movement using a Kalman filter based on a virtual model of movement describing the mechanical properties of the suspension and body, correcting the second data by applying the inverse matrix of the coordinate transformation calculated on the basis of the estimated state vector;

[0022] According to the invention:

[0023] 1. Form an extended Kalman filter state vector, including current motion parameters and motion parameters at previous points in time, corresponding to known time delays in processing optical sensor data:

[0024]

[0025] where vector of parameters of roll, pitch, yaw, vertical displacement and their derivatives; , delays determined by the difference in synchronization timestamps.

[0026] 2. Dynamically scale the measurement noise covariance matrix at each filtering step based on the confidence metric of the semantic image segmentation algorithm:

[0027]

[0028] where – the average probability of image pixels belonging to the “road surface” class, obtained from a convolutional neural network (e.g. UNet, DeepLab); – a small positive number to prevent division by zero; – the base covariance matrix calibrated under nominal conditions.

[0029] 3.Adapt the parameters of the state transition matrix virtual model of real-time motion based on the filter innovation covariance statistics:

[0030]

[0031]

[0032] If for successive steps, adjust the damping coefficients and rigidity models:

[0033]

[0034]

[0035] where , – small adaptation steps selected based on the filter stability condition.

[0036] Advantages of the invention:

[0037] Adaptability to visibility conditions: in fog, night light or dirty lens metric is decreasing, increases, and the filter automatically increases confidence in the physical model (IMU), preventing degradation of the estimate.

[0038] Built-in delay compensation: Extended state vector enables delayed measurements to the corresponding state within a single recursive loop, eliminating the need for external prediction buffering.

[0039] Online model calibration: parameter adaptation Based on innovations, it ensures the stability of the assessment when the vehicle load changes, the suspension wears out, and temperature effects.

[0040] Compatibility with the prototype: The proposed improvements do not require changes to the prototype system architecture, but are implemented as a modification to the motion estimation module software.

[0041] Description of drawings and disclosure of the essence of the invention.

[0042] The features and essence of the claimed invention are explained in the following detailed description, illustrated by the drawings (see Fig. 1-Fig. 9), which show the following.

[0043] Fig. 1. Vehicle sensor motion compensation system;

[0044] Fig. 2. Body sensor motion compensation system (pitch);

[0045] Fig. 3. Block diagram of the on-board computer software (basic);

[0046] Fig. 4. Block diagram of the motion compensation system operation algorithm (main);

[0047] Fig. 5. Architecture with sensor evaluation module;

[0048] Fig. 6. Adaptive filtering and calibration system;

[0049] Fig.7. Timing diagram of the delay compensation system;

[0050] Fig.8. Block diagram of the algorithm of the iterative process with feedback of the motion compensation system;

[0051] Fig.9. Block diagram of the algorithm for predicting future motion with storage of the motion compensation system.

[0052] The method is implemented using an onboard vehicle computer containing a processor (e.g., ARM Cortex-A72, NVIDIA Jetson AGX), RAM and non-volatile memory, sensor connection interfaces (CAN, Ethernet, MIPI CSI-2), and a time stamp synchronization module (PTP, GPS PPS). The sensor complex includes an inertial measurement unit (IMU) with a sampling rate of 200 Hz, a stereo camera with a resolution of 1280×720 and a frequency of 30 Hz, a lidar with an angular resolution of 0.2° and a frequency of 10 Hz, pedal position sensors, steering position sensors, and torque sensors.

[0053] The onboard computer software includes a data preprocessing module for synchronizing sensor timestamps, interpolating IMU data to camera exposure times, normalizing and calibrating raw signals; a semantic segmentation module for a convolutional neural network (e.g., UNet with a ResNet-34-based encoder) trained on the Cityscapes dataset or our own road scene dataset with annotations of the classes "road surface," "marking," and "obstacle," and an output probability map. for each pixel belonging to the "road" class, calculating the confidence metric ; a motion estimation module for implementing an Extended Kalman Filter (EKF) for a nonlinear rotation model, state vector: , transition matrices , which is built on the basis of a linear suspension model with coefficients , adaptable online, measurement matrices , which links the state with IMU readings (angular velocities, accelerations) and the camera (horizon tilt angles, texture offsets); a covariance adaptation module for calculating , limitations of the range of change of diagonal elements to prevent numerical instability; a compensation module for constructing a disturbance matrix based on the assessed , applying the inverse transformation to camera / lidar data , transfer of adjusted data to the road profile construction module; online model calibration module for monitoring the covariance of innovations , when the threshold for adjusting parameters is systematically exceeded , with a step , logging changes for subsequent analysis and further training of the model.

[0054] Mathematical formalization.

[0055] Step 1: Prediction

[0056]

[0057]

[0058] Step 2: Adaptation

[0059]

[0060]

[0061] Step 3: Update

[0062]

[0063]

[0064]

[0065]

[0066]

[0067] Step 4: Adapting the model parameters

[0068] if Then

[0069]

[0070]

[0071] End if

[0072] Step 5: Sensor Data Compensation

[0073]

[0074]

[0075] where , , – rotation matrices around the axes, – vertical displacement matrix.

[0076] The vehicle sensor motion compensation system shown in Figure 1 operates as follows. The vehicle 1 comprises a computing unit 2, including a processor 2a, a memory 2b, a user interface 2c, and an antenna 2d for providing data processing and communication. The computing unit is connected to a signal conditioning unit 3, which comprises a conditioning circuit 3a, an analog-to-digital converter (ADC) 3b, a multiplexer 3c, and a time stamp synchronization unit 3d. The conditioning circuit 3a provides coupling between the sensors and the computing unit, the ADC 3b converts the analog signals of the sensors into digital form, the multiplexer 3c switches multiple input signals, and the time stamp synchronization unit 3d provides time referencing of data from different sensors.Signal matching unit 3 is connected to sensor array 4, which includes camera 4a, lidar 4b, radar 4c, inertial measurement unit (IMU) 4d, gyroscope 4e, and accelerometer 4f. These sensors are designed to obtain information about the vehicle's surroundings and motion parameters. Data from the sensors is received by signal matching unit 3, where it is processed, digitized, and time-synchronized. It is then transmitted to computing unit 2 for further processing to compensate for sensor movements and generate a correct picture of the surrounding space.

[0077] The body sensor motion compensation system shown in Figure 2 operates as follows. Three coordinate systems 5 are defined in the system: the global coordinate system (G) 5a, the unperturbed coordinate system (U) 5b, and the perturbed coordinate system (D) 5c. The xy plane of the global coordinate system is oriented parallel to the road. The perturbation of the coordinate system is characterized by the parameters , representing angular and linear deviations. The measurements obtained by sensor 4a are performed in the disturbed coordinate system (D) 5c. Sensor 4a transmits noisy data to computer 6. Computer 6 performs disturbance compensation by calculating the disturbance transformation matrix as functions of the perturbation parameters. Computer 6 then calculates the correction matrix as the inverse matrix of the perturbation matrix: The corrected input data is calculated by applying the correction matrix to the original noisy data: To estimate the system state parameters and refine the compensation, computer 6 uses an adaptive extended Kalman filter (EKF) 7, which provides an optimal estimate of the disturbance parameters under conditions of measurement uncertainty and noise.

[0078] The on-board computer software block diagram (basic) shown in Figure 3 functions as follows. The process is initiated in the "Start" block. In the first stage, signals are received from sensors 8. Next, the received analog or digital signals are converted into computer inputs 9, suitable for processing by the computing device. The next stage is the categorization of inputs 10, during which the data flow is divided depending on the type of information source. If the data belong to category 11 (inputs 12a, 12b), which includes data from the driver, the inertial measurement unit (IMU) and gyroscopes, the system proceeds to the current motion determination stage 14. The result of this stage is the formation of motion information 15. If the data belong to category 13 (inputs 12), which includes data from cameras, lidars and radars, they are sent directly to the compensation stage.After processing along the appropriate branch, the data is combined and a compensation algorithm 16 is applied to it. This algorithm uses the motion information from category 11 to correct the environmental sensor data in category 13. The result of the algorithm is corrected inputs 17. Next, the corrected inputs 18 are output, which are used to control the operations of the vehicle (TS) 19.

[0079] The motion compensation system algorithm shown in Figure 4 operates as follows. In the first stage, signals are received from sensors 8. These signals are then converted into inputs 9. In the next stage, these inputs are used to determine the current motion of vehicle 14. Based on the received motion data, a compensation algorithm is applied to correct inputs 16. The result of this algorithm is corrected inputs 17, which are output by the system. These corrected inputs are then used to control vehicle 19.

[0080] The architecture with the sensor evaluation module shown in Figure 5 functions as follows. Data is received from sensors 4 by the data acquisition system 20, which forms common inputs 12. The inputs 12 are then divided into two categories depending on the type of data source. The first processing branch includes category 11, which contains data from the IMU and the driver. This data is sent to the motion estimation module 21. The motion estimation module 21 interacts with the virtual model 22, which is connected to the adaptive extended Kalman filter (EKF) 23. The adaptive EKF 23 performs the following functions: provides adaptation for the Confidence Map block from neural network 24, forms an augmented state to compensate for delays 25 and calibrates the parameters in block 26. The second processing branch includes category 13, containing data from cameras and lidars. Motion estimation module 21 also generates information 15, which, together with the data of category 13, is sent to compensation module 27. Based on the received data, compensation module 27 generates corrected inputs 17. Corrected inputs 17 are used to control vehicle 19 and are also transmitted to sensor evaluation module 28. Sensor evaluation module 28 generates data 29 about the road, which is transmitted back to motion estimation module 21, providing feedback for refining the assessment of vehicle motion.

[0081] The adaptive filtering and calibration system shown in Figure 6 operates as follows. The system contains three main functional blocks: a 32-step prediction, a 30-step update, and an adaptation 31 and camera calibration 33. In the prediction step block 32 the motion model 32a generates a predicted state estimate 32c, and the process noise 32b forms the predicted error covariance 32d. Adaptation block 31 provides adaptive adjustment of the measurement noise covariance. The camera image 31a is fed to the input of semantic segmentation 31b, which generates the probability 31c. Based on this probability, the adaptive covariance of the measurement noise is calculated 31d, where base covariance, and a small number to prevent division by zero. In the update step block, 30 dimensions 30a and the observation matrix 30b are used to calculate the covariance of innovations 30c. Based on the covariance of innovations, the Kalman gain is calculated 30d, which is used to update the state assessment 30e and error covariances 30f. Camera Calibration Unit 33 uses the innovation vector 33a for calculating the threshold value 33b, which is used to determine calibration parameters 33c. Calibration data is transmitted back to the adaptation unit and an update step of 30 to refine the filtering process. This system provides adaptive filtering with dynamic adjustment of measurement noise parameters based on semantic image analysis and automatic camera calibration based on an innovative filtering process.

[0082] The timing diagram of the delay compensation system shown in Figure 7 operates as follows. The system operates with multiple time stamps, including the current point in time. 34 and previous points in time 35, 36, 37, where denotes the delay value for the corresponding sensor. Each moment in time corresponds to a system state: 38 for the current moment, 39, 40, 41 for previous moments in time. For each moment in time, the state vector is determined 42, including orientation angles and linear displacements with derivatives The system synchronizes data from various sensors with different time delays: the state for the IMU 43, the state for the camera 44, and the state for the lidar 45. Additional delays 46 are taken into account to compensate for desynchronization between the sensors. Different sensors operate with different characteristics: the IMU operates at a frequency 47 of 100 Hz 50, the camera has a delay 48 of 33 ms 51, and the lidar has a delay 48 of 100 ms 52. The maximum number 49 of delay steps taken into account is 10 steps 53. Thus, the system ensures temporal synchronization of disparate sensors with different sampling rates and delays, which compensates for the effects of vehicle motion between the moments of data acquisition from different sensors.

[0083] The iterative feedback process algorithm of the motion compensation system shown in Figure 8 functions as follows. The process is initiated in the "Start" block. At the initial stage, signals are received from sensors 8, which are then converted into computer inputs 9. Next, the current motion 14 of the vehicle is determined. Based on the received motion data, compensation 16 is performed, after which the processed data 18 are output. The system checks the condition for the need for iteration in block 54. In case of a negative decision (branch "No"), the process goes to the stage of vehicle operation control 19 and is completed in the "End" block. In case of a positive decision (branch "Yes"), sensors 28 are evaluated, followed by data extraction 29. After this, the algorithm returns to the stage of determining the current motion 14, forming an iterative cycle for continuously updating and refining the motion compensation.

[0084] The algorithm for predicting future motion with storage of the motion compensation system shown in Figure 9 functions as follows. The process is initiated in the "Start" block. At the initial stage, signals are received from sensors 8, which are converted into computer inputs 9. Next, the current motion 14 is determined. In parallel with the main processing flow, a motion forecast is performed: a prediction of future motion 55 is performed, and the result is stored in block 56. In the main flow, compensation 16 is performed based on the determined current motion, after which data 18 is output. Next, a measurement of the road ahead is performed 57. In block 58, the system checks the condition for using the stored data. In case of a negative decision (branch "No"), the process goes to the stage of vehicle (TS) operations control 19 and is completed in the "End" block.If the decision is positive (the "Yes" branch), the system performs the "Remember Prediction 56" step, after which the motion estimate 14 is adjusted. The adjusted motion estimate is fed back to the compensation stage 16, closing the feedback loop for refining the compensation.

[0085] Implementation of the invention:

[0086] The claimed invention can be implemented using standard computing technology and sensor equipment used in the modern automotive industry. Below is an example of a specific implementation of a method for adaptive motion compensation of onboard sensors.

[0087] 1. Hardware implementation. The method is implemented using a vehicle's onboard computer (e.g., NVIDIA Jetson AGX or ARM Cortex-A72 platform), connected to the sensor system via CAN, Ethernet, and MIPI CSI-2 interfaces. The sensor system includes:

[0088] - Inertial measurement unit (IMU) containing gyroscopes and accelerometers, with a sampling rate of 200 Hz;

[0089] - Stereo camera with a resolution of 1280×720 pixels and a frame rate of 30 Hz;

[0090] - Lidar with an angular resolution of 0.2° and a frequency of 10 Hz;

[0091] - Time stamp synchronization module (e.g. based on PTP protocol or GPS PPS signal).

[0092] The computing unit contains a processor, RAM and non-volatile memory, in which program instructions for performing the steps of the claimed method are stored.

[0093] 2. Software implementation and sequence of actions. The onboard computer software is modular and includes the following components: a data preprocessing module, a semantic segmentation module, a motion estimation module (EKF), a covariance adaptation module, and a compensation module.

[0094] The system operates in the following order (corresponds to the block diagrams in Fig. 3–Fig. 9):

[0095] Step 1. Synchronization and preprocessing. The system receives initial data from inertial measurement units (IMUs) and secondary data from optical sensors (cameras, lidars). The synchronization module timestamps the data. The IMU data is interpolated to the camera exposure times to align the timescales.

[0096] Step 2. Computer vision confidence assessment. Camera images are fed to a semantic segmentation module (e.g., a UNet convolutional neural network with a ResNet-34 encoder, trained on the Cityscapes dataset or a custom dataset containing annotations for the "road surface," "marking," and "obstacle" classes). For each frame, the ProadProad confidence metric is calculated, defined as the average probability that image pixels belong to the "road surface" class.

[0097] Step 3. Generate an extended state vector. The motion estimation module generates an extended state vector for the Kalman filter. ​, including current movement parameters and parameters at previous points in time to compensate for delays: Where: – roll, pitch and yaw angles, respectively; – vertical movement of the body; – delay determined by the difference in synchronization timestamps (the sum of the exposure time, transmission time, and neural network inference time).

[0098] Step 4. Dynamically adjust the Kalman filter. At each filtering step, the measurement noise covariance matrix is ​​adapted. Scaling is performed using the formula:

[0099]

[0100] where confidence metric (value from 0 to 1); – a small positive number to prevent division by zero; – the base covariance matrix calibrated under nominal conditions.

[0101] Example: When visibility is poor (fog, night) decreases, which leads to an increase The filter automatically reduces the weight of optical measurements and increases confidence in the physical model (IMU).

[0102] Step 5. Adaptation of model parameters. State transition matrix parameters virtual motion models adapt in real time. The system monitors the covariance of filter innovations. If during sequential steps (for example, ) there is a systematic excess of the innovation covariance over the threshold value, the damping coefficients are adjusted and rigidity suspension models:

[0103]

[0104]

[0105] where , – small adaptation steps selected based on the filter stability condition.

[0106] Step 6. Sensor data compensation. Based on the estimated state vector, the disturbance matrix is ​​calculated. and the inverse correction matrix The corrected optical sensor data are calculated as: , Where – the original noisy camera or lidar data. The transformation matrix is ​​constructed as the product of the rotation matrices around the axes and the vertical displacement matrix.

[0107] Step 7. Iterative refinement. The adjusted data is passed to the road profile generation module. The resulting road information is used for feedback and refinement of the traffic estimation in the next cycle (see Fig. 8, Fig. 9).

[0108] 3.Confirmation of technical result

[0109] The possibility of achieving the technical result (increasing the accuracy and robustness of the road surface profile assessment) is confirmed by the following provisions:

[0110] 1. Reducing the Impact of Noise: Using Confidence Metrics for scaling Allows for a mathematically rigorous accounting of image quality. While the prototype (US 10235817 B2) would have used a static sensor, the claimed method increases the variance of camera measurement noise, preventing the state assessment from being "blown" by unreliable visual data.

[0111] 2. Delay Compensation: Including Delayed States in the Vector (Fig. 7) allows the use of delayed measurements to the corresponding state within a single recursive loop. This eliminates the need for external prediction buffering and reduces the computational load, preventing desynchronization during abrupt maneuvers.

[0112] 3. Online calibration: Parameter adaptation Based on innovation statistics, it ensures the stability of the assessment when the vehicle load changes, the suspension wears out or the temperature effects, which is confirmed by the theoretical properties of adaptive Kalman filters.

[0113] The specified technical result is achieved through a combination of features listed in independent claim 1 of the invention formula, in particular due to the formation of an extended state vector (clause 1a), dynamic scaling of the covariance matrix (clause 1b) and adaptation of the model parameters (clause 1c)

[0114] Thus, the described example of implementation demonstrates the possibility of implementing the invention using known hardware when performing specific algorithmic actions aimed at achieving the specified technical result.

[0115] Literature:

[0116] 1. US Patent No. US 10235817 B2. Motion Compensation For On-Board Vehicle Sensors / Martin Saeger, Ashley Elizabeth Micks, Mountain View. Publ. 03 / 19 / 2019.

[0117] 2. "An Introduction to Kalman Filtering Implementation for Localization and Tracking Applications," inHandbook of Position Location: Theory, Practice, and Advances, IEEE, 2019, pp.143-195, doi: 10.1002 / 9781119434610.ch5.

[0118] 3. Ali H. Sayed, "Kalman Filter," in Adaptive Filters , IEEE, 2008, pp.104-110, doi: 10.1002 / 9780470374122.ch12.

Claims

1. A method for compensating for the motion of on-board sensors of a vehicle, comprising: receiving first data from inertial sensors and second data from optical sensors; estimating the motion state vector of the vehicle using a Kalman filter based on a virtual motion model describing the mechanical properties of the suspension and body, and correcting the second data by applying the inverse coordinate transformation matrix calculated on the basis of the estimated state vector, characterized in that the state vector of the Kalman filter is formed as an extended vector including the current motion parameters of the vehicle and the motion parameters of the vehicle at previous moments in time corresponding to known time delays in processing the data of the optical sensors, determined by the difference in synchronization timestamps;the Kalman filter measurement noise covariance matrix is ​​calculated dynamically at each filtering step by scaling the base matrix by a value inverse to the confidence metric of the image processing algorithm, where the confidence metric is defined as the average probability of image pixels belonging to the "road surface" class, obtained from a semantic segmentation convolutional neural network trained on a dataset containing images of road scenes with annotations of the "road surface", "marking", and "obstacle" classes; the parameters of the state transition matrix of the virtual motion model are adapted in real time based on the innovation covariance statistics of the Kalman filter, wherein the adaptation includes adjusting the damping coefficients and vehicle suspension stiffness under the condition that the innovation covariance systematically exceeds a given threshold value.

2. The method according to paragraph 1, characterized in that the convolutional neural network for semantic segmentation has the UNet or DeepLab architecture.

3. The method according to paragraph 1, characterized in that the extended state vector has the form: Where – camera image processing delay, defined as the sum of exposure time, data transfer time, and neural network inference time.

4. The method according to paragraph 1, characterized in that the dynamic scaling of the measurement noise covariance matrix is ​​performed according to the formula: Where – confidence metric, – regularizing parameter, – matrix calibrated under nominal visibility conditions.

5. The method according to paragraph 1, characterized in that the adaptation of the parameters of the state transition matrix is ​​performed with an adaptation step , chosen from the condition of monotonic decrease in the covariance of innovations in a sliding window of length filtration steps.

6. The method according to paragraph 1, characterized in that the inverse matrix of the coordinate transformation is constructed as the product of the rotation matrices around the roll, pitch, yaw axes and the vertical displacement matrix, calculated on the basis of smoothed state estimates from the Kalman filter.

7. A vehicle system comprising: - a computing unit that includes a processor, memory, user interface and antenna to provide data processing and communication, and is associated with - a signal matching unit containing a matching circuit, an analog-to-digital converter (ADC), a multiplexer and a time stamp synchronization module, and connected via connection interfaces to - a sensor block including an inertial measurement unit (IMU), gyroscope, accelerometer, stereo camera, lidar, radar, characterized in that the memory of the computing unit stores program instructions that, when executed by the processor, implement the method according to any of paragraphs 1-6.

8. A machine-readable medium storing a program code executed by the processor of the computing unit of the vehicle, implementing the method according to any of paragraphs 1-6.

Images