Egomotion2.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
 * SPDX-License-Identifier: LicenseRef-NvidiaProprietary
 *
 * NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
 * property and proprietary rights in and to this material, related
 * documentation and any modifications thereto. Any use, reproduction,
 * disclosure or distribution of this material and related documentation
 * without an express license agreement from NVIDIA CORPORATION or
 * its affiliates is strictly prohibited.
 */
 
// Copyright (c) 2022 Mercedes-Benz AG. All rights reserved.
//
// Mercedes-Benz AG as copyright owner and NVIDIA Corporation as licensor retain
// all intellectual property and proprietary rights in and to this software
// and related documentation and any modifications thereto.
// Any use, reproduction, disclosure or distribution of this software and related
// documentation without an express license agreement is strictly prohibited.
//
// This code contains Confidential Information and is disclosed
// under the Mutual Non-Disclosure Agreement.
//
// Notice
// ALL DESIGN SPECIFICATIONS AND CODE ("MATERIALS") ARE PROVIDED "AS IS"
// NO REPRESENTATIONS, WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
// THE MATERIALS, AND EXPRESSLY DISCLAIMS ANY IMPLIED WARRANTIES OF NONINFRINGEMENT,
// MERCHANTABILITY, OR FITNESS FOR A PARTICULAR PURPOSE ARE MADE.
//
// No responsibility is assumed for the consequences of use of such
// information or for any infringement of patents or other rights of third parties that may
// result from its use. No license is granted by implication or otherwise under any patent
// or patent rights. No third party distribution is allowed unless
// expressly authorized.  Details are subject to change without notice.
// This code supersedes and replaces all information previously supplied.
// Products are not authorized for use as critical
// components in life support devices or systems without express written approval.
//
 
// WARNING!!!
// Please don't use any type definition in this file.
// All of data types in this file are going to be modified and will not
// follow Nvidia deprecation policy.
 
#ifndef DW_EGOMOTION_2_0_EGOMOTION2_H_
#define DW_EGOMOTION_2_0_EGOMOTION2_H_
 
#include <dw/egomotion/base/Egomotion.h>
#include <dw/egomotion/base/EgomotionState.h>
#include <dw/egomotion/errorhandling/ErrorHandlingParameters.h>
#include <dw/egomotion/utils/IMUBiasEstimatorParameters.h>
 
#ifdef __cplusplus
extern "C" {
#endif
 
typedef enum dwEgomotionMeasurementTypes {
    DW_EGOMOTION_MEASUREMENT_STEERING             = 0, 
    DW_EGOMOTION_MEASUREMENT_SUSPENSION           = 1, 
    DW_EGOMOTION_MEASUREMENT_ANGULAR_ACCELERATION = 2, 
    DW_EGOMOTION_MEASUREMENT_PROPER_ACCELERATION  = 3, 
    DW_EGOMOTION_MEASUREMENT_ANGULAR_VELOCITY     = 4, 
    DW_EGOMOTION_MEASUREMENT_LINEAR_GROUND_SPEED  = 5, 
    DW_EGOMOTION_MEASUREMENT_LINEAR_BODY_SPEED    = 6, 
    DW_EGOMOTION_MEASUREMENT_WHEEL_TICKS          = 7, 
    DW_EGOMOTION_MEASUREMENT_WHEEL_SPEEDS         = 8, 
    DW_EGOMOTION_MEASUREMENT_TRAILER_ANGLE        = 9, 
    DW_EGOMOTION_MEASUREMENT_TYPES_COUNT
} dwEgomotionMeasurementTypes;
 
typedef enum dwEgomotionGroundSpeedMeasurementTypes {
    DW_EGOMOTION_GROUND_SPEED_FROM_WHEEL_SPEEDS_REAR_AXLE = 0,
    DW_EGOMOTION_GROUND_SPEED_FROM_WHEEL_SPEEDS_BOTH_AXLES = 1,
    DW_EGOMOTION_GROUND_SPEED_FROM_WHEEL_TICKS_AND_SPEEDS_REAR_AXLE = 2,
    DW_EGOMOTION_GROUND_SPEED_FROM_WHEEL_TICKS_AND_SPEEDS_BOTH_AXLES = 3,
    DW_EGOMOTION_GROUND_SPEED_FROM_LINEAR_SPEED = 4,
    DW_EGOMOTION_GROUND_SPEED_COUNT
} dwEgomotionGroundSpeedMeasurementTypes;
 
typedef enum dwEgomotionDirectionMode {
    DW_EGOMOTION_DIRECTION_FROM_SPEED_SIGN = 0,
    DW_EGOMOTION_DIRECTION_FROM_WHEEL_DIR_AND_GEAR = 1,
    DW_EGOMOTION_DIRECTION_FROM_SPEED_SIGN_AND_GEAR = 2,
    DW_EGOMOTION_DIRECTION_COUNT
} dwEgomotionDirectionMode;
 
typedef struct dwEgomotionMeasurementParameters
{
    dwTime_t delay[DW_EGOMOTION_MEASUREMENT_TYPES_COUNT]; 
 
    dwTime_t cycleTime[DW_EGOMOTION_MEASUREMENT_TYPES_COUNT]; 
} dwEgomotionMeasurementParameters;
 
typedef struct dwEgomotionVMOParameters
{
    size_t stateHistorySize; 
 
    float32_t processCovariance[5];        
    float32_t initialProcessCovariance[5]; 
 
    float32_t groundSpeedCovariance[3]; 
    float32_t bodySpeedCovariance[3];   
 
    float32_t chiSquaredInnovationGate; 
 
    float32_t longitudinalGroundSpeedInnovationGate; 
 
    dwTime_t stepSize; 
 
    bool fixedStep; 
 
    dwVector3f referencePoint; 
 
    dwVector3f velocityMax; 
 
    dwVector3f velocityMin; 
} dwEgomotionVMOParameters;
 
typedef struct dwEgomotionWheelObserverParameters
{
    dwVehicleWheelEncoderProperties wheelEncoder;
 
    size_t stateHistorySize; 
 
    dwTime_t stepSize; 
 
    bool fixedStep; 
 
    float32_t processCovariance[3]; 
 
    float32_t positionVarianceLow; 
 
    float32_t positionVarianceHigh; 
 
    float32_t speedVarianceLow; 
 
    float32_t speedVarianceHigh; 
 
    uint32_t positionFuzzyLow; 
 
    uint32_t positionFuzzyHigh; 
 
    float32_t speedFuzzyLow; 
 
    float32_t speedFuzzyHigh; 
 
    float32_t speedMax; 
 
    float32_t accelerationMax; 
 
    float32_t accelerationMin; 
 
} dwEgomotionWheelObserverParameters;
 
typedef struct dwEgomotionVehicleMovingDirectionDetectorParameters
{
    dwEgomotionDirectionMode mode;
 
    dwTime_t durationNoWheelTickThreshold; 
 
    float32_t ticksAmbiguousSpeedThreshold; 
} dwEgomotionVehicleMovingDirectionDetectorParameters;
 
typedef struct dwEgomotionIMUIntrinsics
{
    float32_t gyroscopeBias[3]; 
 
    float32_t accelerometerBias[3]; 
} dwEgomotionIMUIntrinsics;
 
typedef struct dwEgomotionSuspensionProperties
{
    dwVehicleSuspensionProperties properties;
 
    dwVector2f centerHeight; 
} dwEgomotionSuspensionProperties;
 
typedef struct dwEgomotionParameters2
{
    dwGenericVehicle vehicle;
 
    dwEgomotionMeasurementParameters measurements;
 
    dwTransformation3f imuExtrinsics[DW_EGOMOTION_IMU_COUNT_MAX];
 
    dwEgomotionIMUIntrinsics imuIntrinsics[DW_EGOMOTION_IMU_COUNT_MAX];
 
    size_t imuCount;
 
    uint32_t imuSensorIndices[DW_EGOMOTION_IMU_COUNT_MAX];
 
    dwEgomotionSuspensionProperties suspension;
 
    size_t stateHistorySize;
 
    dwEgomotionGroundSpeedMeasurementTypes groundSpeedType;
 
    dwEgomotionWheelObserverParameters wheelObserver;
 
    dwEgomotionVehicleMovingDirectionDetectorParameters vehicleMovingDirectionDetector;
 
    dwEgomotionVMOParameters vehicleMotionObserver;
 
    dwEgomotionErrorHandlingParameters errorHandling;
 
    dwEgomotionIMUAccBiasEstimatorParameters accBiasEstimator;
 
    dwEgomotionIMUGyroBiasEstimatorParameters gyroBiasEstimator;
 
    bool automaticUpdate;
 
    bool enableSuspensionSensorsUsage;
 
} dwEgomotionParameters2;
 
DW_API_PUBLIC
dwStatus dwEgomotion2_initParamsFromRig(dwEgomotionParameters2* params,
                                        dwConstRigHandle_t rigConfiguration,
                                        char const* imu1Name,
                                        char const* imu2Name,
                                        char const* imu3Name);
 
DW_API_PUBLIC
dwStatus dwEgomotion2_initDefaultParams(dwEgomotionParameters2* params);
 
DW_API_PUBLIC
dwStatus dwEgomotion2_initParamsFromRigByIndex(dwEgomotionParameters2* params,
                                               dwConstRigHandle_t rigConfiguration,
                                               uint32_t const imu1Index,
                                               uint32_t const imu2Index,
                                               uint32_t const imu3Index);
 
DW_API_PUBLIC
dwStatus dwEgomotion2_initialize(dwEgomotionHandle_t* const obj,
                                 dwEgomotionParameters2 const* params,
                                 dwContextHandle_t const ctx);
 
DW_API_PUBLIC
dwStatus dwEgomotion2_initializeState(dwEgomotionStateHandle_t* state, size_t const historySize, dwContextHandle_t ctx);
 
DW_API_PUBLIC
dwStatus dwEgomotion2_getAccelerometerBias(dwVector3f* accBias, dwEgomotionConstHandle_t obj);
 
DW_API_PUBLIC
dwStatus dwEgomotion2_getAccelerometerBiasByIndex(dwVector3f* accBias, uint32_t const sensorIndex, dwEgomotionConstHandle_t obj);
 
#ifdef __cplusplus
}
#endif
#endif // DW_EGOMOTION_2_0_EGOMOTION2_H_