Method and system for map building using radar and motion sensors

Abstract

Techniques are disclosed for constructing a map of an area around at least one route traversed by a mobile platform. An integrated navigation solution for a device within the mobile platform is generated using motion sensor data obtained from a sensor assembly of the device and absolute navigation information for the platform. The integrated navigation solution is then used to project radar measurements obtained from at least one radar of the platform onto the area, whereby a map is constructed using the projected radar measurements.

Description

[Technical Field]

[0001] (CROSS-REFERENCE TO RELATED APPLICATIONS) This application claims priority to and the benefit of U.S. Provisional Patent Application No. 63 / 434,625, filed December 22, 2022, and U.S. Patent Application No. 18 / 392,462, filed December 21, 2023, both of which are entitled "METHOD AND SYSTEM FOR POSITIONING USING RADAR AND MOTION SENSORS" and are assigned to the assignee of the present application.

[0002] The present disclosure relates generally to map building techniques, and more particularly to such techniques that employ radar and motion sensor information. [Background technology]

[0003] The development of autonomous platforms, with a particular focus on autonomous vehicles, is an area of ​​much research in academia and industry and has the potential to have a significant impact on daily life. At the forefront of potential applications are autonomous road vehicles. The benefits are manifold, with significant improvements to safety being one of the most important. Unlike human drivers, automated systems are not affected by fatigue or distraction and therefore have much faster reaction times to road conditions, which could dramatically reduce accidents and save lives. In addition, passengers could enjoy more downtime during their commute, during which they could work, socialize, or utilize in-car entertainment systems. Vehicle ownership is expected to decline as it is replaced by ride-sharing, thereby reducing traffic and congestion in urban areas.

[0004] Modern autonomous driving systems require complete knowledge of the surrounding environment. This helps the autonomous system understand various aspects and details about the road. They should be able to acquire and process information in the real-time domain. This includes nearby vehicles, traffic, road speed limits, slow zones, road conditions, road crossing areas, etc. To achieve this, the system requires advanced sensors and technology. A wide variety of applications are emerging that will benefit from autonomy, including transportation trucks, ride-sharing, and passenger cars. Transportation and delivery can benefit from convoys, reduced driver-based service time limits, and greater efficiency. For passenger cars, autonomous driving can free up more time for occupants to be productive during their commute, socialize, or use in-car entertainment systems. Importantly, self-driving technology can dramatically reduce the occurrence of automobile accidents, thereby saving lives. A fundamental challenge in enabling fully autonomous platforms is the need for a ubiquitous, accurate, precise, and reliable navigation system.

[0005] Therefore, there are many available sensors to assist with positioning and navigation problems, each with its own inherent advantages and disadvantages. Therefore, various systems and sensors are required to overcome the limitations of standalone-based systems and achieve reliable navigation in all environments and conditions. For example, conventional systems used for navigation estimation include inertial measurement units (IMUs) and global navigation satellite systems (GNSSs). IMU-based systems provide accurate relative attitude estimation in a short time. However, due to the mechanization process, standalone IMU-based systems, such as inertial navigation systems (INSs), accumulate state errors exponentially over time. On the other hand, the position estimated by a GNSS receiver is absolute and does not drift over time. However, GNSS signals can be completely blocked or affected by severe multipath. Due to the complementary error characteristics of motion sensors and GNSS systems, the conventional approach to accurately estimate vehicle attitude is to integrate IMU and GNSS signals using sensor fusion algorithms. The performance of GNSS systems can also be improved by using differential GPS stations that can broadcast ionospheric and tropospheric errors to nearby GNSS receivers.

[0006] Perception sensors can provide a wealth of information when combined with high-definition (HD) maps. Fusing such sensors with an inertial navigation system (INS) can enable object detection and position updates from map matching. The most common sensors include cameras and lidar. Cameras are typically low-cost, have small form factors, and provide dense information about the environment, such as object color and texture. However, both types of sensors are significantly affected by adverse weather conditions, and cameras can also be adversely affected by backlit objects and lack of illumination from the scene. Additionally, cameras do not provide direct measurements of range and speed. These inherent weaknesses reduce the reliability and availability of cameras and lidar.

[0007] An alternative perception sensor found in automobiles today is radar. Key advantages of radar include its robustness against adverse weather conditions, its insensitivity to lighting variations, and its ability to provide long, accurate range measurements. They can also be packaged behind optically opaque vehicle panels, thereby providing industrial designers with a degree of flexibility not possible with other perception sensors. Automotive imaging radars have lower resolution than lidar, but recent advances are narrowing the gap. The current generation of state-of-the-art automotive imaging radars provides high-rate information about multiple dynamic targets in highly cluttered scenes in a 4D domain consisting of range, Doppler, azimuth, and elevation measurements. To leverage these advantages, tightly coupled navigation modules integrate multiple radars and HD maps with inputs from INS and global navigation satellite systems and odometry to provide accurate, high-speed, and continuous (always available) navigation solutions in all environments and conditions. Support for flexible numbers and configurations of imaging radars enables the navigation module to provide lane-level accuracy solutions to meet the high-precision requirements of both road vehicles and wheeled robotic platforms.

[0008] Thanks to recent technological advances, imaging radar can now provide high-resolution information about multiple dynamic targets in highly cluttered scenes with high update rates. Current state-of-the-art automotive imaging radars can return high-resolution information of the environment in the 4D domain (range, Doppler, azimuth, and elevation). Despite these advantages, radar still presents challenges to HD map matching localization techniques due to sparse data and lower angular resolution than lidar. Furthermore, the integration of multi-radar configurations effectively extends the radar field of view, providing wider, or even 360-degree, horizontal coverage while maintaining high scan rates. The increased coverage and more detections can help the HD map matching process achieve better results. Such systems have been shown to be effective in enabling accurate and reliable navigation systems for both vehicle and robotic platforms in GNSS-degraded or GNSS-denied environments. Beyond its use for localization, multi-radar configurations can also be effective tools for imaging scenes, especially when modern sensors with higher angular and range resolutions are used.

[0009] HD maps in 2D and 3D forms, including different formats such as occupancy grid maps and point clouds, are used as one of the primary sources to enable solutions from different perception sensors. HD maps can be generated using different sensors, such as lidar, cameras, or radar. Positioning solutions can also use radar-based maps generated by crowdsourcing techniques across the mapped area. Using crowdsourcing techniques, multiple passes of the same area can be aggregated to create a crowdsourced radar map of the environment. As the platform moves through the area, radar detections are projected to an absolute position based on the current navigation state. Radar detections are accumulated over trajectories over time. This technique can be particularly useful in multi-radar configurations of two or more radars, where the radars are geometrically positioned to achieve a combined 360-degree azimuth view. Combining data from multiple passes of the same area using crowdsourcing techniques results in a final crowdsourced map. It is generally preferable to collect crowdsourced data at various times of day to remove non-permanent features such as parked cars. To remove non-permanent features, detected features from multiple passes through the same area, preferably at different times of day, can then be aggregated to form a crowdsourced HD map, which can then be used in subsequent runs as a global reference map for localization purposes.

[0010] Building 2D / 3D maps for autonomous vehicles or robots outdoors / indoors is a very challenging problem. One challenge that can affect map building is the availability of real-time kinematic (RTK) GNSS. RTK GNSS signal quality impacts positioning solutions when signals are either unavailable or of very poor quality, especially in urban areas or GNSS-denied environments such as underground or covered parking garages. Another challenge is that most map building techniques require the use of highly sensitive laser sensors or other high-precision sensors, making the process very costly and financially expensive because most techniques are based on collecting data using multiple sensors. The cost of map building can also include data storage if such sensors collect data at a very high rate and with a large number of points or detections, requiring a large portion of memory to store the data. Furthermore, data processing is also an item that affects the cost of map building, including data transfer, manipulation, and processing. Therefore, the use of low-cost imaging radar allows for a cost vs. quality trade-off for map-building requirements where maps can be built using a single imaging radar or multiple imaging radars. This work applies to any perception sensor, such as a visual sensor.

[0011] Maps can play an important role in improving navigation solutions, especially in difficult environments where the navigation solution may be impaired in such areas due to multipath and signal disruption / jamming. Different navigation techniques that use radar / vision can benefit from maps to provide state updates to the navigation solution. Maps can be constructed in 2D or 3D domains, and these maps can be used to enhance the navigation solution.

[0012] What is needed is a technique for providing an estimate of the confidence and quality of positioning in an area of ​​a crowdsourced map based on the quality of the map in that area and the quality of the navigation solution used to build the map. In accordance with the techniques of this disclosure, radar measurements can be used along with information from motion sensors during the map building process, as described in the following documents: Summary of the Invention

[0013] The present disclosure includes a method for constructing a map of an area around at least some route traversed by the mobile platform using an integrated navigation solution for a device within the mobile platform, the method involving acquiring motion sensor data from a sensor assembly of the device, acquiring absolute navigation information about the platform, acquiring radar measurements from at least one radar of the platform, generating an integrated navigation solution based at least in part on the motion sensor data and the absolute navigation information, the integrated navigation solution providing at least a position and orientation output, projecting the radar measurements onto the area from the position and orientation outputs of the integrated navigation solution, and constructing a map for the area using the projected radar measurements.

[0014] The present disclosure also includes a system for constructing a map of an area around at least one route traversed by the mobile platform using an integrated navigation solution for a device in the mobile platform, the system may include a device having a sensor assembly configured to output motion sensor data, a source of absolute navigation information, at least one radar configured to output radar measurements of the platform, and at least one processor coupled to acquire the motion sensor data and the radar measurements, the processor operative to: generate an integrated navigation solution that provides at least a position and orientation output based at least in part on the motion sensor data and the absolute navigation information; project the radar measurements onto the area from the position and orientation output of the integrated navigation solution; and construct a map for the area using the projected radar measurements. [Brief explanation of the drawings]

[0015] [Figure 1] FIG. 1 is a schematic diagram of a device for providing a navigation solution by integrating radar measurements with motion sensor data, according to one embodiment. [Figure 2] FIG. 1 is a schematic diagram of another device architecture for providing a navigation solution by integrating radar measurements with motion sensor data, according to one embodiment. [Figure 3] FIG. 1 is a schematic diagram of the disclosed technique for map building, according to one embodiment. [Figure 4] FIG. 10 is a schematic diagram of an exemplary routine for constructing a map by integrating absolute navigation information and motion sensor data and projecting radar measurements based on determined positions to provide a navigation solution for a device in a mobile platform, according to one embodiment. [Figure 5] FIG. 1 is a schematic diagram of a routine for using data sessions from different routes according to one embodiment. [Figure 6]10A and 10B illustrate a schematic representation of subdivision into slices with probabilities according to one embodiment; [Figure 7] 1 illustrates an example of an accuracy map that may be derived from the acquired information, according to one embodiment. [Figure 8] 1 illustrates an alternative configuration of a radar sensor, according to an embodiment. [Figure 9] 1 illustrates different geometric detection patterns depending on the radar sensor configuration employed, according to one embodiment. [Figure 10] 10A and 10B are schematic representations of radar measurements taken when a detection point is at the edge of an area, according to one embodiment; [Figure 11] 1 shows a schematic representation of a subdivision of an area into cells and slices, with corresponding confidence determinations for these subdivisions, according to one embodiment; [Figure 12] 12A and 12B schematically illustrate a subsection of FIG. 11 with only cells / slices within the area with detected points, according to one embodiment. [Figure 13] 12A and 12B schematically illustrate a subsection of FIG. 11 in which only cells / slices that the platform has passed through are given an estimated confidence, according to one embodiment. [Figure 14] 10 illustrates examples of different geometries and different precisions, according to one embodiment. [Figure 15] 10 illustrates examples of different geometries and different precisions, according to one embodiment. [Figure 16] 10 illustrates examples of different geometries and different precisions, according to one embodiment. [Figure 17] 10 illustrates examples of different geometries and different precisions, according to one embodiment. [Figure 18] 10 illustrates examples of different geometries and different precisions, according to one embodiment. [Figure 19] 10 illustrates examples of different geometries and different precisions, according to one embodiment. [Figure 20] 10 illustrates examples of different geometries and different precisions, according to one embodiment. [Figure 21]10 illustrates examples of different geometries and different precisions, according to one embodiment. [Figure 22] 10 illustrates examples of different geometries and different precisions, according to one embodiment. [Figure 23] 10 is a further example of map construction, according to one embodiment. [Figure 24] 10 is a further example of map construction, according to one embodiment. [Figure 25] 10 is a further example of map construction, according to one embodiment. [Figure 26] 10 is a further example of map construction, according to one embodiment. [Figure 27] 10 is a further example of map construction, according to one embodiment. [Figure 28] 10 is a further example of map construction, according to one embodiment. [Figure 29] 10 is a schematic representation of an example of map construction, according to one embodiment; [Figure 30] 10 is a schematic representation of an example of map construction, according to one embodiment; [Figure 31] 10 is a schematic representation of an example of map construction, according to one embodiment; [Figure 32] 10 is a schematic representation of an example of map construction, according to one embodiment; [Figure 33] 10 is a schematic representation of an example of map construction, according to one embodiment; [Figure 34] 10 is a schematic representation of an example of map construction, according to one embodiment; [Figure 35] 10 is a schematic representation of an example of map construction, according to one embodiment; [Figure 36] 10A-B illustrate a schematic diagram of improved accuracy through integration of radar measurements, according to one embodiment; [Figure 37] 10A-B illustrate a schematic diagram of improved accuracy through integration of radar measurements, according to one embodiment; DETAILED DESCRIPTION OF THE INVENTION

[0016] It should be understood at the outset that this disclosure is not limited to the specifically exemplified materials, architectures, routines, methods, or structures, as such may vary. Thus, although several such options similar or equivalent to those described herein can be used in the practice or embodiments of this disclosure, the preferred materials and methods are described herein.

[0017] It is also to be understood that the terminology used herein is for the purpose of describing particular embodiments of the disclosure only, and is not intended to be limiting.

[0018] The detailed description set forth below in connection with the accompanying drawings is intended as a description of exemplary embodiments of the present disclosure and is not intended to represent the only exemplary embodiments in which the present disclosure may be practiced. The term "exemplary" as used throughout this description means "serving as an example, instance, or illustration" and should not necessarily be construed as preferred or advantageous over other exemplary embodiments. The detailed description includes specific details for the purpose of providing a thorough understanding of the exemplary embodiments herein. It will be apparent to those skilled in the art that the exemplary embodiments herein may be practiced without these specific details. In some instances, well-known structures and devices are shown in block diagram form to avoid obscuring the novelty of the exemplary embodiments presented herein.

[0019] For convenience and clarity only, directional terms such as top, bottom, left, right, above, below, overhead, upward, downward, directly below, rear, aft, and front may be used in connection with the accompanying drawings or chip embodiments. These and similar directional terms should not be construed as in any way limiting the scope of the present disclosure.

[0020] In this specification and claims, when an element is referred to as being "connected" or "coupled" to another element, it will be understood that the element may be directly connected or coupled to the other element, or intervening elements may be present. In contrast, when an element is referred to as being "directly connected" or "directly coupled" to another element, there are no intervening elements present.

[0021] Some portions of the detailed descriptions which follow are presented in terms of procedures, logic blocks, processes, and other symbolic representations of operations on data bits within a computer memory. These descriptions and representations are the means used by those skilled in the data processing arts to most effectively convey the substance of their work to others skilled in the art. In the present application, a procedure, logic block, process, or the like, is conceived to be a self-consistent sequence of steps or instructions leading to a desired result. The steps are those requiring physical manipulations of physical quantities. Usually, though not necessarily, these quantities take the form of electrical or magnetic signals capable of being stored, transferred, combined, compared, and otherwise manipulated in a computer system.

[0022] It should be borne in mind, however, that all of these and similar terms are to be associated with the appropriate physical quantities and are merely convenient labels applied to these quantities. As will become apparent from the description that follows, unless otherwise expressly stated, throughout this application, descriptions utilizing terms such as "access," "receive," "send," "use," "select," "determine," "normalize," "multiply," "average," "monitor," "compare," "apply," "update," "measure," "derive," and the like will be understood to refer to operations and processes of a computer system or similar electronic computing device that manipulate and convert data represented as physical (electronic) quantities in the computer system's registers and memory into other data similarly represented as physical quantities in the computer system's memory or registers or other such information storage, transmission, or display devices.

[0023] The embodiments described herein may be described in the general context of processor-executable instructions residing on some form of non-transitory processor-readable medium, such as program modules, executed by one or more computers or other devices. Generally, program modules include routines, programs, objects, components, data structures, etc. that perform particular tasks or implement particular abstract data types. The functionality of the program modules may be combined or distributed as desired in various embodiments.

[0024] While a single block may be illustrated in the figures as performing one or more functions, in reality, the function or functions performed by that block may be implemented in a single component or across multiple components, and / or may be implemented using hardware, software, or a combination of hardware and software. To clearly illustrate this interchangeability of hardware and software, various example components, blocks, modules, circuits, and steps have been described above generally in terms of their functionality. Whether such functionality is implemented as hardware or software depends on the particular application and design constraints imposed on the overall system. Those skilled in the art may implement the described functionality in various ways for each particular application, and such implementation decisions should not be interpreted as causing a departure from the scope of the present disclosure. Additionally, an example wireless communication device may include components other than those shown, including well-known components such as a processor, memory, etc.

[0025] The techniques described herein may be implemented in hardware, software, firmware, or any combination thereof, unless specifically described as being implemented in a particular manner. Any features described as modules or components may also be implemented together in an integrated logic device or separately as discrete but interoperable logic devices. If implemented in software, the techniques may be realized at least in part by a non-transitory processor-readable storage medium comprising instructions that, when executed, perform one or more of the methods described above. The non-transitory processor-readable data storage medium may form part of a computer program product, which may include packaging materials.

[0026] Non-transitory processor-readable storage media may include random access memory (RAM), such as synchronous dynamic random access memory (SDRAM), read-only memory (ROM), non-volatile random access memory (NVRAM), electrically erasable programmable read-only memory (EEPROM), flash memory, other known storage media, and the like. Additionally or alternatively, the technology may be realized at least in part by a processor-readable communication medium that carries or communicates code in the form of instructions or data structures and that can be accessed, read, and / or executed by a computer or other processor. For example, a carrier wave may be employed to carry computer-readable electronic data such as those used in sending and receiving email or in accessing a network such as the Internet or a local area network (LAN). Of course, many modifications may be made to this configuration without departing from the scope or spirit of the claimed subject matter.

[0027] The various illustrative logic blocks, modules, circuits, and instructions described in connection with the embodiments disclosed herein may be executed by one or more processors, such as one or more motion processing units (MPUs), digital signal processors (DSPs), general-purpose microprocessors, application specific integrated circuits (ASICs), application specific instruction set processors (ASIPs), field programmable gate arrays (FPGAs), or other equivalent integrated or discrete logic circuitry. As used herein, the term “processor” may refer to any of the foregoing structures or any other structure suitable for implementing the techniques described herein. Additionally, in some aspects, functionality described herein may be provided in dedicated software modules or dedicated hardware modules configured as described herein. The techniques may also be implemented entirely within one or more circuits or logic elements. A general-purpose processor may be a microprocessor, but alternatively, the processor may be any conventional processor, controller, microcontroller, or state machine. A processor may also be implemented as a combination of computing devices, such as a combination of an MPU and a microprocessor, a plurality of microprocessors, one or more microprocessors in conjunction with an MPU core, or any other such configuration.

[0028] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure pertains.

[0029] Finally, as used in this specification and the appended claims, the singular forms "a," "an," and "the" include plural referents unless the content clearly dictates otherwise.

[0030] The technology of this disclosure is directed to map building by using motion sensors and absolute navigation information to generate an integrated navigation solution that can be correlated with radar measurements of a platform. Typically, a platform is a wheeled vehicle or other similar vessel intended for use on land, but may also be used at sea or in the air. Thus, a platform may also be referred to as a vehicle. However, a platform may also be a pedestrian or walking user. As will be appreciated, motion sensor data includes information from an accelerometer, gyroscope, or IMU. Inertial sensors are self-contained sensors that use gyroscopes to measure rotational / angular rates and accelerometers to measure specific forces (from which acceleration is derived). Inertial sensor data can be used in an INS, a non-reference-based relative positioning system. Using an initial estimate of the mobile platform's position, velocity, and orientation angle as a starting point, INS readings can then be integrated over time and used to determine the platform's current position, velocity, and orientation angle. Typically, measurements are integrated once for gyroscopes to obtain an orientation angle and twice for accelerometers to obtain the platform's position incorporating the orientation angle. Thus, gyroscope measurements undergo three integration operations in the process of yielding position. However, inertial sensors alone are not suitable for precise positioning because the required integration of the data results in a position solution that drifts over time, thereby leading to unbounded accumulation of error. Integrating absolute navigation information, such as from GNSS, with motion sensor data can help mitigate such errors.

[0031] A device contained within a platform (which, as mentioned above, may be any type of vehicle or watercraft, and in some applications, may be a person) may have one or more sources of navigation or position information. In some embodiments, the device is strapped or restrained in a fixed orientation relative to the platform. A device is “strapped,” “strapped,” or “tethered” to the platform when it is physically connected to the platform in a fixed manner that does not change over time during navigation; for a strapped device, the relative position and orientation between the device and the platform does not change over time during navigation. In particular, in a strapped configuration, the attachment of the device to the platform is assumed to be in a known orientation. Nevertheless, in some situations, the intended attachment orientation may shift, resulting in misalignment between the device and the platform. In one aspect, the techniques of the present disclosure may be employed to characterize and correct such attachment misalignment.

[0032] In other embodiments, a device is considered “unstrapped” or “untethered” when the device has some mobility with respect to (or within) the platform, meaning that the relative position or orientation between the device and the platform may change over time during navigation. Under these conditions, the device's relative orientation with respect to the platform may change, which is sometimes referred to as misalignment. Similar to the mounting misalignment described above, this varying misalignment between the device's frame and the platform's frame may be determined and compensated for accordingly using the techniques of this disclosure. A device may be “unstrapped” in two scenarios: when the device's mobility within the platform is “unconstrained” or when the device's mobility within the platform is “constrained.” An example of “unconstrained” mobility may be a person traveling on foot and holding a portable device such as a smartphone in their hand (which may also move) for text input or browsing purposes, holding it to their ear, dangling / waving it in their hand, holding it in a belt clip, or in their pocket, among other use cases; such use cases may change over time, and further, in each use case, the orientation relative to the user may change. Another example of "unconstrained" mobility of a device within a platform is a person on a boat or vehicle who holds a portable device such as a smartphone in their hand (the hand can also move) for text entry or browsing purposes, against their ear, in a belt clip, or in their pocket, among other use cases, and such use cases may change over time, and furthermore, each use case may have a changing orientation relative to the user. An example of "constrained" mobility may be when a user enters a vehicle and places a portable device (such as a smartphone) in a rotatable holder or cradle. In this example, the user may rotate the holder or cradle at any point during navigation, thus changing the orientation of the device relative to the platform or vehicle.Thus, when unstrapped, the device's mobility may be constrained or unconstrained within the platform, and it may be moved or tilted in any orientation within the platform, and the techniques of the present disclosure may still be applied under all of these conditions. Accordingly, some embodiments described below include portable handheld devices that can be moved in space by a user and thus sense their movement, position, and / or orientation within space. The techniques of the present disclosure may function with any type of portable device as desired, including smartphones or other exemplary devices described below. It will be appreciated that such devices are often carried by or associated with a user and, therefore, may benefit from providing navigation solutions using a variety of inputs. For example, such a handheld device may be a mobile phone (e.g., a cellular phone, a phone operating on a local network, or any other telephone handset), a tablet, a personal digital assistant (PDA), a video game player, a video game controller, a navigation device, a wearable device (e.g., eyeglasses, a watch, a belt clip), a fitness tracker, a virtual or augmented reality device, a mobile internet device (MID), a personal navigation device (PND), a digital still camera, a digital video camera, binoculars, a telephoto lens, a portable music player, a video player, or a media player, a remote control, or other handheld device, or a combination of one or more of these devices. However, the techniques of this disclosure may also be applied to other types of devices that are not handheld, including devices that are integrated with or may be used in conjunction with autonomous or piloted vehicles, whether land, air, or underwater vehicles.By way of example only and not limitation, the platform may be a drone, also known as an unmanned aerial vehicle (UAV).

[0033] To help illustrate aspects of the present disclosure, features of a suitable device 100 are shown in FIG. 1 with high-level schematic blocks. As will be appreciated, device 100 may be implemented as a device or apparatus, such as a strapped, unstrapped, tethered, or untethered device as described above, and when unstrapped, the device's mobility may be constrained or unconstrained within the platform, and it may be moved or tilted to any orientation within the platform. As shown, device 100 includes a processor 102, which may be one or more microprocessors, central processing units (CPUs), or other processors for executing software programs, which may be stored in memory 104 and associated with the functionality of device 100. Multiple layers of software may be provided within memory 104, which may be any combination of computer-readable media, such as electronic memory, or other storage media, such as a hard disk, optical disk, etc., for use with processor 102. For example, an operating system layer may be provided on device 100 to control and manage system resources in real time, enable application software and other layer functions, and interface application programs with other software and functions of device 100. Similarly, different software application programs may be provided, such as menu navigation software, games, camera function control, navigation software, communication software such as telephony or wireless local area network (WLAN) software, or any of a wide variety of other software and function interfaces. In some embodiments, multiple different applications may be provided on a single device 100, and in some of these embodiments, multiple applications may be running simultaneously.

[0034] Device 100 includes at least one sensor assembly 106 for providing motion sensor data representative of device 100's movement in space. The sensor assembly 106 includes inertial sensors such as accelerometers and gyroscopes; other motion sensors may also be used, including magnetometers, pressure sensors, or others. Depending on the configuration, the sensor assembly 106 measures one or more axes of rotation and / or one or more axes of acceleration of the device. In one embodiment, the sensor assembly 106 may include an inertial rotational motion sensor or an inertial linear motion sensor. For example, the rotational motion sensor may be a gyroscope that measures angular velocity along one or more orthogonal axes, and the linear motion sensor may be an accelerometer that measures linear acceleration along one or more orthogonal axes. In one aspect, three gyroscopes and three accelerometers may be employed such that a sensor fusion operation performed by processor 102 or other processing resources of device 100 combines data from the sensor assembly 106 to provide a six-axis determination of movement, or six degrees of freedom (6DOF). Furthermore, the sensor assembly 106 may include a magnetometer measuring along three orthogonal axes, and output data to be fused with gyroscope and accelerometer inertial sensor data to provide a nine-axis determination of motion. Similarly, the sensor assembly 106 may also include a pressure sensor to provide an altitude determination that may be fused with other sensor data to provide a ten-axis determination of motion. If desired, the sensor assembly 106 may be implemented using a Micro Electro Mechanical System (MEMS), allowing integration into a single, compact package.

[0035] Optionally, device 100 may implement an additional sensor assembly in the form of external sensor 108, which may represent one or more of the sensors described above, such as inertial motion sensors (i.e., accelerometers and / or gyroscopes), other motion sensors, or other types of sensors. For example, in some of the embodiments described below, external sensor 108 is an auxiliary sensor, such as an optical camera, a thermal camera, an infrared imaging sensor, a light detection and ranging (lidar) system, or other suitable sensor that records images or samples to help classify objects detected by a radar system. As used herein, "external" refers to a sensor that is not integrated with sensor assembly 106 and may be remote or local to device 100. Alternatively or additionally, sensor assembly 106 and / or external sensor 108 may also be configured to measure one or more other aspects of the environment surrounding device 100. This is optional and not required in all embodiments. For example, a pressure sensor and / or a magnetometer may be used to refine motion determination. Although described in the context of one or more sensors being MEMS-based, the techniques of this disclosure may be applied to any sensor design or implementation.

[0036] In the illustrated embodiment, the processor 102, memory 104, sensor assembly 106, and other components of device 100 may be coupled via bus 110, which may be any suitable bus or interface, such as a peripheral component interconnect express (PCIe) bus, a universal serial bus (USB), a universal asynchronous receiver / transmitter (UART) serial bus, a suitable advanced microcontroller bus architecture (AMBA) interface, an Inter-Integrated Circuit (I2C) bus, a serial digital input output (SDIO) bus, a serial peripheral interface (SPI), or other equivalents. Depending on the architecture, different bus configurations may be employed as needed. Additional buses may be used to couple various components of device 100, such as by using a dedicated bus between the processor 102 and memory 104.

[0037] As described in detail below, the techniques of the present disclosure involve integrating radar measurements with motion sensor data provided by sensor assembly 106 (or other sensors, such as external sensor 108) to provide a navigation solution. Device 100 obtains radar measurements 112 from any suitable radar system, which may be integrated with device 100, associated with or connected to device 100, part of a platform, or implemented in any other desired manner.

[0038] Algorithms, routines, or other instructions for processing sensor data may be employed by the integration module 114 to perform any of the operations associated with the techniques of this disclosure. In one aspect, an integrated navigation solution based on motion sensor data and absolute navigation information may be output by the integration module 114. As used herein, a navigation solution (also referred to as a navigation state) includes at least a position and may also include an attitude (also referred to as an orientation) and / or a velocity. Thus, a navigation solution may be position only, or position and attitude (orientation), or position and velocity, or position, velocity, and attitude (orientation), or may also include other quantities (or states). Determining the navigation solution may involve sensor fusion or similar operations performed by the processor 102, which may use the memory 104, or any combination of other processing resources. The orientation may be only a heading angle (which may be an azimuth angle or yaw), or the orientation may also include other orientation angles and components, such as pitch and / or roll. For example, the orientation may simply be a heading, or may be a 3D orientation such as roll, pitch, and heading, or any other representation of a 3D orientation (such as a quaternion or rotation vector, among other representations).

[0039] Correspondingly, device 100 also has sources of absolute navigation information 116, such as Global Navigation Satellite System (GNSS) receivers, including, but not limited to, Global Positioning System (GPS), Global Navigation Satellite System (GLONASS), Galileo, and / or Beidou, as well as WiFi™ positioning, cellular tower positioning, Bluetooth™ positioning beacons, or other similar methods of deriving a navigation solution. Integration module 114 may also be configured to use information from wireless communication protocols to provide a navigation solution determination using signal trilateration. Any suitable protocol may be employed, including cellular-based and wireless local area network (WLAN) technologies, such as Universal Terrestrial Radio Access (UTRA), Code Division Multiple Access (CDMA) networks, Global System for Mobile Communications (GSM), Institute of Electrical and Electronics Engineers (IEEE) 802.16 (WiMAX), Long Term Evolution (LTE), IEEE 802.11 (WiFi™), and others. A source of absolute navigation information represents a "reference-based" system that relies on external information sources, as opposed to self-contained navigation information provided by a self-contained system within a device / platform, such as sensor assembly 106, and / or a "non-reference-based" system, as described above.

[0040] In some embodiments, device 100 may include a communications module 118 for any suitable purpose, including transmitting map construction derived as the platform passes through an area. Communications module 118 may employ a wireless local area network (WLAN) conforming to the Institute of Electrical and Electronics Engineers (IEEE) 802.11 protocol, which may feature multiple transmit and receive chains to increase bandwidth and achieve greater throughput. For example, the 802.11ad (WiGIG™) standard includes the ability for devices to communicate in the 60 GHz frequency band over four 2.16 GHz-wide channels, delivering data rates of up to 7 Gbps. Other standards may also involve the use of multiple channels operating in other frequency bands, such as the 5 GHz band, or may use other systems, including Universal Terrestrial Radio Access (UTRA), Code Division Multiple Access (CDMA) networks, Global System for Mobile Communications (GSM), IEEE 802.16 (WiMAX), Long Term Evolution (LTE), other transmission control protocols, cellular-based technologies such as Internet Protocol (TCP / IP) packet-based communications, and WLAN technologies. In some embodiments, multiple communication systems may be employed to take advantage of different capabilities. Typically, communications with higher bandwidth may be associated with greater power consumption, such that other channels may utilize lower power communication protocols, such as BLUETOOTH, ZigBee, ANT, etc. Furthermore, wired connections may also be employed. In general, communications may be direct or indirect, such as through one or more interconnected networks. As will be appreciated, a variety of systems, components, and network configurations, topologies, and infrastructures, such as client / server, peer-to-peer, or hybrid architectures, may be employed to support a distributed computing environment. For example, computing systems may be connected together by wired or wireless systems, by local networks or widely distributed networks.Many networks are currently connected to the Internet, which provides an infrastructure for widely distributed computing and encompasses many different networks, although any network infrastructure may be used for the exemplary communications accompanying the techniques described in the various embodiments.

[0041] As will be appreciated, the processor 102 and / or other processing resources of the device 100 may be one or more microprocessors, central processing units (CPUs), or other processors that execute software programs for the device 100 or other applications related to the functionality of the device 100. For example, different software application programs such as menu navigation software, games, camera function control, navigation software, and phone calls, or a wide variety of other software and function interfaces, may be provided. In some embodiments, multiple different applications may be provided on a single device 100, and in some of these embodiments, multiple applications may run simultaneously on the device 100. Multiple layers of software may be provided on a computer-readable medium, such as electronic memory or other storage media, for use with the processor 102, such as a hard disk, optical disk, flash drive, etc. For example, an operating system layer may be provided on the device 100 to control and manage system resources in real time, enable the functionality of the application software and other layers, and interface the application programs with other software and functionality of the device 100. In some embodiments, one or more motion algorithm layers may provide motion algorithms for low-level processing of raw sensor data provided from internal or external sensors. Additionally, a sensor device driver layer may provide a software interface to hardware sensors of device 100. Some or all of these layers may be provided in memory 104 or in any other suitable architecture for access by processor 102. Embodiments of the present disclosure may feature any desired partitioning of processing between processor 102 and other processing resources as appropriate for the application and / or hardware employed.Aspects implemented in software may include, but are not limited to, application software, firmware, resident software, microcode, etc., and may take the form of a computer program product accessible from a computer-usable or computer-readable medium that provides program code for use by or in connection with a computer or any instruction execution system, such as processor 102, a dedicated processor, or any other processing resource of device 100.

[0042] As another illustration of aspects of the present disclosure, features of different device architectures are shown in FIG. 2 with high-level schematic blocks in the context of device 200. Here, device 200 includes a host processor 202 and memory 204, similar to the embodiments described above. Device 200 includes at least one sensor assembly for providing motion sensor data, shown here in the form of an integrated motion processing unit (MPU®) 206 or any other sensor processing unit (SPU), featuring a sensor processor 208, memory 210, and internal sensors 212. Memory 210 may store algorithms, routines, or other instructions for processing data output by internal sensor 212 and / or other sensors described below using the logic or controller of sensor processor 208, and may also store raw data and / or motion data output by internal sensor 212 or other sensors. Memory 210 may also be used for any of the functions associated with memory 204. Internal sensor 212 may be one or more sensors for measuring the movement of device 200 in space as described above, including inertial sensors such as accelerometers and gyroscopes; in addition, other motion sensors may be used, including magnetometers, pressure sensors, or others. Exemplary details regarding suitable configurations of host processor 202 and MPU 206 may be found in commonly owned U.S. Pat. No. 8,250,921, issued Aug. 28, 2012, and U.S. Pat. No. 8,952,832, issued Feb. 10, 2015, which are incorporated herein by reference in their entireties. A suitable implementation of MPU 206 in device 200 is available from InvenSense, Inc., San Jose, California.

[0043] Optionally, device 200 may implement another sensor assembly in the form of external sensors 214, which may represent sensors such as the inertial motion sensors (i.e., accelerometers and / or gyroscopes) described above, other motion sensors, or other types of sensors. In this context, "external" refers to sensors that are not integrated with MPU 206 and may be remote or local to device 200. Also, alternatively or additionally, MPU 206 may receive data from auxiliary sensors 216 configured to measure one or more aspects of the environment surrounding device 200. This is optional and not required in all embodiments. For example, pressure sensors and / or magnetometers may be used to refine the motion determination made using internal sensors 212. In the illustrated embodiment, the host processor 202, memory 204, MPU 206, and other components of the device 200 may be coupled via a bus 218, while the sensor processor 208, memory 210, internal sensors 212, and / or auxiliary sensors 216 may be coupled via a bus 220, any of which may be any suitable bus or interface as described above.

[0044] Again, the techniques of the present disclosure involve integrating absolute navigation information with motion sensor data provided by internal sensor 212 (or other sensors) to provide a navigation solution, whereby radar measurements 222 from the integrated radar system may be projected over the area for which a map is being constructed. Alternatively, the radar system may be associated with device 200, may be part of the platform, or may be implemented in any other desired manner. Also, similar to the above embodiment, algorithms, routines, or other instructions for processing sensor data, including integrating radar measurements 222, may be employed by integration module 224 to perform any of the operations associated with the techniques of the present disclosure. Determining a navigation solution may involve sensor fusion or similar operations performed by MPU processor 208. In other embodiments, some or all of the processing and calculations may be performed by host processor 202, which may use any combination of host memory 204 or other processing resources.

[0045] Thus, device 200 may have a source of absolute navigation information 226 and may include a communications module 228 for any suitable purpose. The source of absolute navigation information 226 and / or communications module 228 may have any of the characteristics described above with respect to the source of absolute navigation information 116 and communications module 118.

[0046] Similar to processor 102, host processor 202 and / or sensor processor 208 may be one or more microprocessors, central processing units (CPUs), or other processors that execute software programs for device 200 or other applications related to the functionality of device 200. Embodiments of the present disclosure may feature any desired partitioning of processing between host processor 202, MPU 206, and other processing resources as appropriate for the applications and / or hardware employed.

[0047] As described, the techniques of the present disclosure use radar measurements to build a map of the environment surrounding the platform and the device. In one aspect, sensors, such as sensors integrated with devices in the platform, continuously log real-time measurements to derive locations that can be correlated with radar measurements as part of the map-building operation. As used herein, a map is a global representation of all elements / objects in the real world that can affect the measurements logged by the sensors. A map can be either static or dynamic, depending on whether only static objects are represented on the map, or whether static and dynamic objects are represented on the map. Two well-known forms of maps include location-based maps and feature-based maps.

[0048] In location-based maps, the map is represented as a set of objects in a set m, where m i =m ix,iy,iz The ith object, represented by m, is its 3D location in the map. ix,iy,izis the Cartesian coordinate represented by the ith element. Each position object can include other attributes that describe the object. A notable property of a position-based map is that the list of objects in set m is indexed by their position instead of any other attributes. The main advantage of a position-based map is that all positions in the map are represented, and therefore the map has a complete description of empty and non-empty positions in the map. A well-known example of a position-based map is the Occupancy Grid Map (OGM), in which the real world is discretized into squares (for 2D maps) or cubes (for 3D maps). Objects in an OGM map are the positions of the center points of the squares / cubes, and each position object can have several attributes. For example, one attribute can reflect whether the square / cube is occupied or empty (alternatively, this attribute can reflect whether the square / cube is occupied, empty, or unmapped), and another attribute can include the expected measurement vector of a particular sensor at the current position object.

[0049] In feature-based maps, the map is represented as a set of objects in a set m, where m iThe ith object represented by m is a particular feature object in the map. In other words, a feature-based map is a collection of objects that somehow represent certain features in the environment. These objects usually have several attributes, including the object's location. A notable property of a feature-based map is that only selective locations of the environment are represented in m. A feature-based map can be either sparse or dense, depending on the number of feature objects across the map. Furthermore, the feature objects can be uniformly distributed (or any other distribution) across different locations in the map, or the feature objects can be densely clustered at certain locations. Finally, the uniqueness of each of the feature objects is another property of a feature map. These properties affect how useful a feature map can be for localization purposes. A feature-based map consisting of dense, unique, and uniformly distributed feature objects (across locations in the map) is generally a desirable property for a localization system.

[0050] In the context of this disclosure, either the device or platform also includes a radar sensor system that provides radar measurements 112 or 222. Radar has a number of characteristics based on the signal used by the sensor, the area / volume covered by the radar, the accuracy and resolution of the radar range / orientation, and the types of measurements logged by the sensor. Suitable characteristics and types of radar sensors are discussed in the following documents:

[0051] Most radar sensors emit one of two types of RF signals: pulse-based signals and Frequency Modulated Continuous Wave (FMCW) signals. Pulse-based radar emits signals at frequencies cA radar employs a single carrier frequency, represented by , and transmits this frequency in the form of repeated pulses. This type of radar sensor switches between transmitting a pulse and then waiting for a short period of time. The radar uses silent periods to receive reflections of the transmitted pulse (if the pulse is reflected by a target). By applying a Fast Fourier Transform (FFT) to the repeated pulses in the time domain, very large signal bandwidths can be achieved. The range and Doppler resolutions of pulse-based radar are proportional to the radar pulse width and carrier frequency, respectively.

[0052] On the other hand, FMCW signals are continuous in time, so their bandwidth is limited. FMCW signals are generated by radar sensors by linearly modulating the frequency over time with a Voltage Controlled Oscillator (VCO). This process is called sweeping. Furthermore, SW BD The sweep bandwidth, denoted by SW, is determined by the band in which the radar operates. T The sweep time, represented by SW BD The time is set to vary linearly over a period of time. FMCW-based radars typically transmit and receive simultaneously (duplex), so the sensor does not stop transmitting to receive reflections from the target.

[0053] As for which radar technology is better for the technology of this disclosure, each type has its advantages and disadvantages. Pulse-based radar does not require complex calculations like FMCW-based radar. Furthermore, there is no Doppler range coupling, as is the case with some FMCW-based modulation schemes (triangular sweeps can help efficiently separate both measurements). In addition, the accuracy of FMCW-based radar depends on the linearity of the VCO employed. However, pulse-based radar leaks power into adjacent bands, limiting frequency reuse. This is a limitation of pulse-based radar, especially in urban areas where autonomous vehicles operate in close proximity. Because FMCW-based radar is continuous wave, it does not leak significant power out of band, thus enabling frequency reuse and limiting interference. Another disadvantage of pulse-based radar is the need for transmit and receive times. In mission-critical scenarios where autonomous vehicles are operating at high speeds or in dense urban environments, the inability to transmit and receive simultaneously results in significant latency, which in turn hinders real-time updates. Nevertheless, several manufacturers are commercializing pulse-based radars that offer simultaneous transmission and reception.

[0054] Another important characteristic of radar systems is the frequency operating band employed. There are several millimeter frequency bands used by radar sensors. The two main operating bands are 24 GHz and 77 GHz. The 24 GHz band is in the Industrial, Scientific, and Medical radio (ISM) band. 24 GHz-based radars operate from 24.0 GHz to 24.25 GHz, which corresponds to a bandwidth of 250 MHz. On the other hand, the 77 GHz band offers a wider bandwidth of up to 4 GHz. The range accuracy and range resolution of radars increase as the available sweep bandwidth increases. Range resolution reflects the radar's ability to detect clustered / close targets. Furthermore, higher operating frequencies result in higher velocity accuracy and velocity resolution. 77 GHz-based radars operate at a larger bandwidth (providing 20 times better range accuracy and range resolution) and a higher frequency (providing 3 times better velocity accuracy and velocity resolution) compared to 24 GHz-based radars. Finally, the dimensions of radar antennas are proportional to the operating frequency. The antenna for a 77GHz radar is three times smaller than a 24GHz antenna.

[0055] Regardless of the type of signal or operating band used by the radar, the radar signal processing unit estimates range and Doppler from the received reflections. The radar range at a particular azimuth and elevation angle is given by r α,β where α is the azimuth / bearing angle relative to the radar coordinates and β is the elevation angle.

[0056]

number

[0057]

number

[0058]

number

[0059]

number

[0060] Other characteristics of a radar system are its range and beamwidth. Radars can be classified based on their range and field of view (FOV). This results in three types of radar: long-range radar (LRR), medium-range radar (MRR), and short-range radar (SRR). LRR in the 76-77 GHz band with a bandwidth of 600 MHz typically covers a range of 10-250 m, a range resolution and accuracy of 0.5 m and 0.1 m, respectively, a velocity resolution and accuracy of 0.6 m / s and 0.1 m / s, respectively, an angular accuracy of 0.1 degree, a 3 dB beamwidth at + / - 15 degrees azimuth, and a 3 dB beamwidth at + / - 5 degrees elevation. MRRs in the 77-81 GHz band with a 600 MHz bandwidth typically cover a distance range of 1-100 m, distance resolution and accuracy of 0.5 m and 0.1 m, respectively, velocity resolution and accuracy of 0.6 m / s and 0.1 m / s, respectively, angular accuracy of 0.5 degrees, 3 dB beamwidth at + / -40 degrees azimuth, and 3 dB beamwidth at + / -5 degrees elevation. Finally, SRRs in the 77-81 GHz band with a 4 GHz bandwidth typically cover a distance range of 0.15-30 m, distance resolution and accuracy of 0.1 m and 0.02 m, respectively, velocity resolution and accuracy of 0.6 m / s and 0.1 m / s, respectively, angular accuracy of 1 degree, 3 dB beamwidth at + / -80 degrees azimuth, and 3 dB beamwidth at + / -10 degrees elevation.

[0061] Generally, increasing the radar bandwidth for the same (or similar) operating frequency can result in better range accuracy and range resolution. However, a 4 GHz bandwidth is only used for SRR. Even though better range accuracy and range resolution are desirable for LRR, the radar's distance range is proportional to its transmit power. If a higher transmit power is used by a radar operating at a wide bandwidth, this can cause very high out-of-band interference. Therefore, a compromise can be made between the radar's distance range (how far its signal can reach) and its range and accuracy. Furthermore, the velocity resolution and velocity accuracy of different types of radar are substantially equivalent because they all operate around the same frequency band.

[0062] Finally, the field of view (FOV) of a radar is equal to the beamwidth in azimuth and elevation. The radars described so far have a wider FOV in azimuth compared to elevation, but some manufacturers are also working on 3D radars with wider azimuth and elevation beamwidths. Furthermore, some automotive radars can scan medium and long ranges simultaneously.

[0063] According to the present disclosure, techniques are provided for constructing a map using imaging radar and motion sensor data. The map is constructed using radar measurements projected from the position and orientation of an integrated navigation solution. The area to be mapped is divided into subsections to address memory growth from accumulating map points across the area. Examples of subsections may be tiles, slices, and cells, among others. The subsections may be at various levels, either single-level subsections or multi-level subsections. For example, in the case of multi-level subsections, a map of an area may first be subdivided into tiles, which may be subdivided into slices, which may be subdivided into cells. In particular, a potential position accuracy (confidence) of the constructed map may be provided using different features of the map, data from orbit, and / or sensor accuracy. Confidence is used for any of the types of map subsections described above. The confidence of a map subsection represents the potential position accuracy that can be obtained from that subsection of the map if the map is used for subsequent positioning. In particular, determining confidence in this context involves estimating the potential accuracy of future position fixes using an already constructed map. This confidence is estimated when the map is constructed based on factors such as those described above. This confidence helps evaluate how good or useful the constructed map may be. As a result of this confidence measure associated with a map subsection, more or less confidence may be placed in that map when it is subsequently used to obtain future navigation solution(s). Generally, the technique involves acquiring trajectory data, making navigation and mapping decisions, and constructing a map, as shown in FIG. 3. Data is collected using different configurations of radar, which can use a single radar or multiple radars, and the radars can be forward-facing and rear-facing on a vehicle. Different types of maps can be constructed, such as 2D maps and 3D maps.The system can create maps with or without certain features such as traffic lights and bridges. The system can also update the maps to filter out unwanted objects such as parked cars. The maps are built using data crowdsourcing techniques, where data is collected from different routes through the area of ​​interest.

[0064] As described above, acquiring trajectory data is the first step in map construction. After identifying the area to be mapped, several routes or trajectories for data collection during transit may be proposed to adequately cover the mapped area. The proposed routes may take into account factors such as the sensors involved in route data collection, the number of trajectories to be collected for each route, the direction of the route, the division of the route into groups or phases, and the number of phases required to conduct data collection to cover the entire area, as well as other suitable information. The collected data includes measurements from RTK GNSS, IMU, odometer, and imaging radar. Radar measurements are projected using a solution from INS / GNSS sensor fusion. Data can be collected according to a plan and surveyed to obtain data from the trajectories. Additionally, data can be collected as crowdsourced data collected from different vehicles during their normal (daily) activities, and data can be collected and aggregated from different platforms.

[0065] In the next step, a navigation solution is determined along the trajectory so that the obtained radar measurements can be projected onto the area being mapped. The projection process uses the navigation solution to project all detections. The solution can be obtained from the navigation module using different sensors. It can be obtained using only GNSS / INS or using a radar / GNSS / INS-based solution as described below. An integrated navigation solution can provide a seamless navigation solution. When an accurate RTK GNSS solution is available, the system can integrate RTK-GNSS with the INS to correct sensor errors and calibrate the INS. To help reduce error growth during outage intervals, different sensors such as an odometer and barometer are added to the integrated solution. The integrated solution provides a continuous, reliable, and accurate solution consisting of at least position, and optionally speed, attitude, and heading, along with all of their accuracy measures.

[0066] To achieve an integrated solution from multiple sensors and systems, different state estimation techniques can be used. Some possible examples of state estimation techniques are the Kalman Filter (KF), the Linearized Kalman Filter (LKF), the Extended Kalman Filter (EKF), the Unscented Kalman Filter (UKF), and the Particle Filter (PF), among others. The above filters or other state estimation techniques include a prediction phase and an update phase (sometimes called the measurement update phase). The state estimation technique also uses a system model and measurement model(s) based on which measurements are used. The system model is used in the prediction phase, and the measurement model(s) are used in the update phase. In the full-state approach, state estimation or filtering techniques estimate the state of the device itself (such as the device's position, velocity, and attitude), and the system model or state transition model used is the kinematic model itself. In the case of inertial navigation, the kinematic model is a nonlinear model, and this model is a full-state model because the estimated state is the state of the navigation device itself. In the error-state approach, the kinematic model is used externally in the so-called inertial mechanization, which is a nonlinear model as described above, and the output of this model is the navigation state of the module, such as position, velocity, and attitude. State estimation or filtering techniques estimate errors in the navigation state obtained by the mechanization, and therefore the estimated state vector by this state estimation or filtering technique is relative to the error state, and the system model is an error-state system model that transitions the previous error state to the current error state. The mechanization output is corrected for these estimated errors to provide a corrected navigation state, such as a corrected position, velocity, and attitude. The estimated error state is relative to a nominal value, which is the mechanization output; the mechanization can operate unassisted in open-loop mode, or it can receive feedback from the corrected state, in which case it is called closed-loop mode.Furthermore, the architecture of the state estimation technique of the present disclosure can be loosely coupled, tightly coupled, or very tightly coupled (also known as deeply coupled). In a loosely coupled architecture, a separate navigation solution is derived using inertial sensor data and one or more auxiliary sources of navigation information (i.e., measurements, these are measurements used in the measurement model), such as a GNSS-, visual-, and / or radar-based positioning solution, or any other source of navigation information that provides a position / velocity solution. The outputs of these separate solutions are then combined in the position / velocity domain in an integrated navigation solution, referred to as a loosely coupled integrated navigation solution. In a tightly coupled architecture, a typical state estimation technique directly integrates inertial sensor readings with raw data (raw measurements that are not in the position domain) from one or more auxiliary sources of navigation information. As an example, for GNSS data, this can include pseudoranges, which can be generated from a combination of code or carrier phase, or both, and pseudorange rates, which can be calculated from Doppler shifts; for radar, this can include radar measurements (radar detections) themselves (not radar-based positioning). In an ultra-tightly coupled architecture, an auxiliary source of navigation information also uses feedback from an inertial navigation system to improve performance.

[0067] In the final stage, radar measurements are used to build a map. Data is collected using multiple radars in conjunction with INS / GNSS. The system can use RTK-GNSS to improve the accuracy / quality of the GNSS data. The mapping operation uses the navigation solution from the INS / GNSS integration to project radar detections onto a positioning domain. The result is a local map representing all detections from the imaging radar. Radar detections can be projected based on positioning from the navigation solution, and positions can be from the INS / GNSS integration alone or from the INS / GNSS / radar integration solution. Each trajectory generates a map with all points from the radar data collected during that trajectory session. Building a global map can involve crowdsourcing techniques to aggregate all maps from all acquired trajectories. Maps can be built using a collection of data sessions from different routes.

[0068] To help illustrate the techniques of the present disclosure, FIG. 4 shows an example routine for integrating absolute navigation information and motion sensor data to provide a navigation solution for a device in a mobile platform and constructing a map by projecting radar measurements based on the determined position. While described in the context of device 100 as shown in FIG. 1, other architectures, including the architecture shown in FIG. 2, may be used with appropriate modifications as desired. First, motion sensor data for device 100 may be acquired, such as from sensor assembly 106. In one aspect, the sensor data may be inertial sensor data from one or more accelerometers, gyroscopes, or other suitable motion- and / or orientation-detecting sensors. Absolute navigation information 116 for the platform is also acquired. As the platform traverses its orbit, radar measurements 112 are also acquired. Correspondingly, an integrated navigation solution may be generated based at least in part on the motion sensor data and absolute navigation information to provide at least a position output. The radar measurements are then projected such that a map for the area is constructed using the projected radar measurements.

[0069] Thus, a preferred method involves obtaining motion sensor data from a sensor assembly of the device, obtaining absolute navigation information about the platform, obtaining radar measurements from at least one radar of the platform, generating an integrated navigation solution that provides at least a position output based at least in part on the motion sensor data and the absolute navigation information, projecting the radar measurements from the position output of the integrated navigation solution onto an area, and constructing a map for the area using the projected radar measurements.

[0070] In one aspect, constructing the map may include aggregating projected radar measurements for multiple position and orientation outputs of the integrated navigation solution along at least one route.

[0071] In one aspect, projected radar measurements may be propagated for at least one of: a plurality of position and orientation outputs of an integrated navigation solution along a plurality of routes; a plurality of position and orientation outputs of an integrated navigation solution from a plurality of mobile platforms; and a plurality of position and orientation outputs of an integrated navigation solution from a plurality of mobile platforms along a plurality of routes.

[0072] In one aspect, a confidence level may be determined for a subsection of an area of ​​a map constructed based at least in part on the plurality of position and orientation outputs, the determined confidence level representing the potential accuracy of the position output of another integrated navigation solution subsequently derived using the constructed map.

[0073] In one aspect, an uncertainty may be determined for the generated integrated navigation solution used in constructing the map, and a determined confidence level for a subdivision of an area of ​​the map is based at least in part on the determined uncertainty.

[0074] In one aspect, the determined reliability of a subsection of an area of ​​a map may be based at least in part on absolute navigation information. In one particular aspect, the determined reliability of a subsection of an area of ​​a map may be based at least in part on the accuracy (also referred to as uncertainty) of the absolute navigation information. For example, the accuracy (or uncertainty) of the absolute navigation information may be used in at least one of the following: a) the accuracy of the absolute navigation information may be used in aggregate from all routes that a mobile platform takes through the subsection of the map, and b) the accuracy of the absolute navigation information may be used around the subsection of the map from each separate route that takes through the subsection of the map.

[0075] In one aspect, the determined confidence level of a subdivision of an area of ​​the map may be based at least in part on a geometry that depends on the configuration of the platform's at least one radar.

[0076] In one aspect, the determined confidence level of a subdivision of an area of ​​the map may be based at least in part on the number of features detected using the projected radar measurements.

[0077] In one aspect, the determined reliability of a subdivision of an area of ​​the map may be based at least in part on a route traversed by the mobile platform.

[0078] In one aspect, the determined confidence level of a subdivision of an area of ​​the map may be based at least in part on one of: a) all subdivisions of the area; b) subdivisions of the area having detection results determined from projected radar measurements; and c) subdivisions of the area along at least one route traversed by the mobile platform.

[0079] In one aspect, at least one route may be configured to provide desired coverage of an area.

[0080] In one aspect, when generating an integrated navigation solution, absolute navigation information may be rejected when degradation is detected.

[0081] In one aspect, the integrated navigation solution may be based on at least one of forward processing, reverse processing, and a combination of forward and reverse processing.

[0082] In one aspect, the integrated navigation solution may be based on a smoothing process.

[0083] In one aspect, constructing the map may involve at least one of using the projected radar measurements to assign new probability decisions to subdivisions of areas of the map and updating existing probability decisions for subdivisions of areas of the map.

[0084] In one aspect, occupancy probabilities may be determined for subdivisions of areas of a map.

[0085] In one aspect, the map may be cleaned based at least in part on trajectories passing through subdivisions of the area of ​​the map.

[0086] In one aspect, the output of the integrated navigation solution may be improved based at least in part on the received motion sensor data using a nonlinear state estimation technique, wherein a prediction phase involving a system model is used to propagate predictions regarding the state of the platform, and an update phase involving at least one measurement model relating measurements to the state is used to update the state of the platform, the nonlinear state estimation technique including using a nonlinear measurement model for radar measurements, wherein the integration of the motion sensor data and the radar measurements in the nonlinear state estimation technique is tightly coupled, and generating i) using the acquired motion sensor data in a non-linear state estimation technique; ii) directly integrating radar measurements by updating a nonlinear state estimation technique using a nonlinear measurement model and the constructed map; The output of the improved integrated navigation solution is used to project radar measurements over an area to build an improved map.

[0087] In one aspect, the measurement model comprises: i) a radar range-based model based at least in part on a probability distribution of measured ranges using the estimated state of the platform and the constructed map; ii) a radar nearest object likelihood model based at least in part on a probability distribution of distances to objects detected using radar measurements, an estimated state of the platform, and nearest object identifications from the constructed map; and iii) a radar map matching model based at least in part on a probability distribution derived by correlating a global map derived from the constructed map with the projected radar measurements and the map constructed using the integrated navigation solution; and and iv) a radar closed-form model based at least in part on the relationship between the integrated navigation solution and the range to the object from the constructed map.

[0088] According to the present disclosure, a suitable system may include a device having a sensor assembly configured to output motion sensor data; a source of absolute navigation information; at least one radar configured to output radar measurements of the platform; and at least one processor coupled to obtain the motion sensor data and the radar measurements, the at least one processor operative to: generate an integrated navigation solution that provides at least a position and orientation output based at least in part on the motion sensor data and the absolute navigation information; project the radar measurements onto an area from the position and orientation output of the integrated navigation solution; and construct a map for the area using the projected radar measurements.

[0089] In one aspect, the at least one processor may be operative to construct a map by aggregating projected radar measurements for multiple position and orientation outputs of the integrated navigation solution along at least one route; i) multiple position and orientation outputs of the integrated navigation solution along multiple routes; ii) multiple position and orientation outputs of the integrated navigation solution from multiple mobile platforms; and iii) a plurality of position and orientation outputs of the integrated navigation solution from a plurality of moving platforms along a plurality of routes.

[0090] In one aspect, the at least one processor may further operate to determine a confidence level for a subsection of an area of ​​a map constructed based at least in part on the plurality of position and orientation outputs, the determined confidence level representing a potential accuracy for a position output of another integrated navigation solution subsequently derived using the constructed map.

[0091] In one aspect, the at least one processor may be further operative to use a nonlinear state estimation technique to improve a position output of the integrated navigation solution based at least in part on the motion sensor data, wherein a prediction phase involving a system model is used to propagate predictions regarding a state of the platform, and an update phase involving at least one measurement model relating measurements to the state is used to update the state of the platform, the nonlinear state estimation technique including using a nonlinear measurement model for radar measurements, wherein the integration of the motion sensor data and the radar measurements in the nonlinear state estimation technique is tightly coupled, and generating i) using the acquired motion sensor data in a non-linear state estimation technique; ii) directly integrating radar measurements by updating a nonlinear state estimation technique using a nonlinear measurement model and the constructed map; The output of the improved integrated navigation solution is used to project radar measurements over an area to build an improved map.

[0092] In one aspect, the sensor assembly includes an accelerometer and a gyroscope. Additionally, the sensor assembly may be implemented as a micro-electromechanical system (MEMS). [Example]

[0093] It is contemplated that the present methods and systems may be used for any application involving constructing a map of an area around at least one route traversed by a mobile platform. An integrated navigation solution for a device within the mobile platform is used to project radar measurements onto the area to construct the map. Without being limited to the above, the present disclosure is further illustrated by the following examples.

[0094] When constructing a global map or crowdsourced map, the map construction module aggregates all maps from all acquired trajectories. The map can be constructed using data sessions from different routes, as shown in Figure 5. Each route can cover a portion of the area to be mapped. The area to be mapped is divided into subdivisions, such as tiles, slices, and cells, to accommodate memory growth from accumulating map points across the area. Each tile is divided into slices to handle different portions from different routes. The map area is defined based on the maximum and minimum ranges of the collected trajectories. The area is initially established based on the first trajectory and then expanded using any additional trajectories with wider dimensions. The map construction technique generates cells (e.g., 10 x 10 cm) and combines all points located in each cell into a single point. Points from all maps are aggregated, and their probability values ​​are updated based on the point locations. All points located in a cell are represented by a single point.

[0095] If a point is new to a cell, it is generated with its probability value. If the point is an addition to an existing point in a cell, the probability is added to the existing point probability. The point (cell) total probability is strengthened or weakened based on the number of occurrences where all collected trajectories pass through the same area. The route / trajectory area is divided and represented as equal sized cells (e.g., 10x10cm), and each cell is represented as occupied or not based on the number of located detections within this area.

[0096] When merging from different crowdsourced areas occurs, a slicing technique can be used to clean the map to remove most of the unnecessary points and maintain the different areas. Each tile is divided into slices, and the slices are set to update the probability of all points located within any cell in the slice, as shown in Figure 6. The slice count depends on the number of trajectories passed and the number of points located inside its boundary. It will be understood that the slicing technique updates the point occupancy probability based on the number of trajectories and the number of points that can increase or decrease the point probability value.

[0097] In some embodiments, a GNSS rejection routine is implemented. The integrated navigation solution described above can filter out some of the GNSS degradation. However, severe GNSS degradation over a longer duration can erroneously pull and drag the solution. To avoid such degradation to the integrated navigation solution, a GNSS automatic rejection method is implemented to reject GNSS positions in areas with severe GNSS degradation. Examples of severe GNSS degradation include areas of high GNSS multipath. GNSS rejection techniques can be based on detecting degradations such as multipath, cycle slips, and poor position and / or velocity standard deviation values. Furthermore, the integrated navigation solution can be used for rejection of severe GNSS degradation by evaluating the persistence of discrepancies between the GNSS solution and the integrated navigation solution.

[0098] In other embodiments, the accuracy of the navigation solution may be improved using forward / reverse processing. Specifically, the positioning and navigation solution may be a forward integrated navigation solution as described above. Alternatively, the forward / reverse solution may be used either constantly during a navigation session or partially as needed during a navigation session. In the latter case of using the forward / reverse solution only partially as needed, this may be based on factors such as the accuracy of the forward navigation solution, the GNSS positioning and / or signal quality, and the availability of GNSS signals, among others.

[0099] The navigation module performs forward processing of the input data to derive a forward navigation solution for each epoch, while performing backward processing of the input data to derive a reverse navigation solution for the epoch using information obtained subsequent to the current epoch for which the navigation solution is determined. A smoothed navigation solution is provided by combining the navigation solution quantities from the forward and backward directions. Smoothing generally refers to combining values ​​for the navigation solution quantities obtained from the forward and backward processing. If desired, additional time-based smoothing techniques are available to reduce changes in values ​​for the navigation solution quantities between different times. Such smoothing techniques may be non-causal because they can utilize information from future as well as past periods for any given epoch. For example, a time-based smoothing process may be used to smooth height data obtained from a barometer. If available, absolute navigation information may be used to determine a barometer offset, which may be used to compensate for the smoothed solution. Similarly, a time-based smoothing process may also be employed for quantities including position, velocity, and speed. Additionally, smoothing operations that may be performed on height employ absolute navigation information and / or motion sensor data. Height information may be determined from a barometer, but the data may be corrupted by noise and bias. For example, absolute navigation information, such as from a GNSS system, may be used to estimate the offset or bias of the barometer data.

[0100] To improve accuracy during map construction, an area may be divided into subdivisions, such as tiles, and each subdivision may be further divided into slices and cells, as appropriate. As one non-limiting example, the cell size may be set to 10 cm x 10 cm, while the slice is set to 10 m x 10 m. Correspondingly, maps can be constructed for on-road and off-road areas. The proposed method indicates a confidence (which is the potential positioning accuracy) for an area, subdivision, or cell of the crowdsourced map. This confidence represents the potential positioning accuracy that can be obtained for subsequent future navigation solutions obtained by using this map. The method can provide two confidence maps (also called accuracy maps), one per cell (10 cm x 10 cm) and one per slice (10 m x 10 m). In an additional optional embodiment, the confidence can be estimated using the position uncertainty of different INS / GNSS crowdsourced routes. Navigation solutions can be from INS / GNSS only or from INS / GNSS / perception.

[0101] The potential positioning accuracy (confidence) for an area or cell in a crowdsourced map may be calculated based on any suitable criteria. In one embodiment, this may comprise a geometric shape seen within a shape based on a radar range radius centered on that cell. The aforementioned cells are for traversable areas. Alternatively or additionally, the criteria may be the GNSS accuracy around the cell. This may involve aggregating the GNSS accuracy from all orbits around the cell of interest. For example, the GNSS accuracy may be used around the aforementioned cell for each orbit used to build the map. Furthermore, the position uncertainty of the integrated navigation solution may be used to generate a point confidence. The estimated confidence may be determined from the routes used for data crowdsourcing of the mapped area.

[0102] Therefore, the techniques of the present disclosure can provide accuracy maps at different resolutions. For example, accuracy maps can be provided at a resolution of one per cell (10 cm x 10 cm) and one per slice (10 m x 10 m). The confidence of an area / slice / cell can be estimated as the average of all aggregated point confidences within the same cell / slice. An exemplary diagram is shown schematically in Figure 7.

[0103] In another aspect, the configuration of radar sensors used to acquire measurements about the platform may be defined based on different geometric shapes, as shown in FIG. 8. As will be appreciated, the configuration of radar sensors depends on the number, orientation, and field of view (FOV). Thus, a geometric area is used to calculate the confidence of a cell / slice at the center of the geometric illustration. For ease of illustration and description, a rectangular shape is used in the following examples. However, in addition to the general form of geometric area shown in FIG. 9, different geometric shapes may be employed, along with their associated geometric shapes, to locate detection points at the edge of the area, as shown in FIG. 10.

[0104] As described above, an integrated navigation solution that includes the platform's position may have an associated reliability estimate. Different techniques may be used when determining the reliability, as desired. For example, the reliability may be estimated using all cells / slices within an area, as shown in FIG. 11. Alternatively, only cells / slices within an area with detected points may be estimated, as shown in FIG. 12. In yet another embodiment, only cells / slices within an area that the platform has passed through may be given an estimated reliability, as shown in FIG. 13. To implement these techniques for a slice, the slice through which the platform has traveled may be considered. Alternatively, an appropriate range around the orbit may be used to determine which cells should be used.

[0105] From the above, it will be appreciated that potential positioning accuracy (confidence) can be affected based on features within a geometric area, as shown in the following examples of areas with different confidence levels. For example, the geometry of an area can have different levels of accuracy based at least on the number of features detected. As one non-limiting example, the geometry can be classified using categories such as poor, below average, average, above average, and good, among others. Poor geometry can result from poor coverage of the area, where not enough routes pass through the area, especially at the edges of the mapping area. It can also result from insufficient features in the area itself and not having enough detections to help properly shape the area. Furthermore, poor geometry can occur in areas with open space, such as parking lots, sports courses, or large athletic fields.

[0106] To aid in explanation, Figures 14-22 provide examples of areas with different geometries and different accuracy levels, with the complete map shown in the top view and a sliced ​​view below for comparison. In particular, Figures 14 and 15 show areas with poor geometries. Next, Figure 16 shows an area with poor geometries, targeting a street view of a parking lot area with cones and poles. In particular, the fences and poles provide criteria that can be extracted, as shown in the center map. Next, examples of average geometries are provided in Figures 17-19, which show typical diagrams. Next, good geometries are illustrated in the examples shown in Figures 20-22.

[0107] If desired, the map-building process can be improved using a two-step technique. Two-step refers to performing the map-building process in two phases / runs. The first phase / run is to build a map using only INS / GNSS solutions, without any perception or maps. This run generates a first version of the crowdsourced map. The second phase / run provides solutions from INS / GNSS / perception based on the map obtained from the first run. The map obtained from the second run is the desired map of the crowdsourced area. This technique improves the quality of the crowdsourced map, especially in areas with poor GNSS signals due to multipath.

[0108] In these embodiments, radar measurements can be directly integrated when determining a navigation solution. Thus, the state estimation technique of the present disclosure uses a measurement model for the radar measurements such that the acquired radar measurements directly update the state estimation technique, which may be nonlinear. The nonlinear model does not suffer from approximation or linearization and can improve the navigation solution of the device when using very low-cost, low-end inertial sensors. The radar measurement model(s) are nonlinear. The system model may be linear or nonlinear. The system model may be a linear or nonlinear error-state system model. Traditional linear state estimation techniques, such as a Kalman filter (KF) or an extended Kalman filter (EKF), require linearized approximations. By avoiding the need for linearized approximations, the nonlinear technique of the present disclosure can provide a more accurate navigation solution for the device.

[0109] Further examples are provided in FIGS. 23-37. Specifically, FIG. 23 shows a route to be traversed within the area of ​​downtown Calgary, California, that can be used to construct a map according to the techniques of this disclosure. Correspondingly, FIG. 24 shows a map generated for this area. The quality of the constructed map can be affected based on a number of different factors, including, but not limited to, the number of sensors, the sensor configuration, and the number of routes used to cover the mapped area. Accordingly, FIG. 25 shows how the mapped area can be subdivided, such as into tiles as shown. Furthermore, one trajectory within this area is schematically represented in FIG. 26, and the generated slice is shown in FIG. 27. Next, FIG. 28 shows only the slices that contain detections, thus eliminating any zero-detection slices.

[0110] As another example, Figures 29-35 correspond to an area of ​​downtown Detroit mapped using the techniques of the present disclosure. Specifically, Figure 29 shows one route that may be traversed to determine locations and correlate radar measurements with those determined locations, while Figure 30 shows multiple routes that may be traversed using crowdsourcing techniques. A map constructed using the crowdsourced routes of Figure 30 is shown in Figure 31. Accordingly, the map may be subdivided into tiles as shown in Figure 32. Thus, the route shown in Figure 33 represents a trajectory that the platform may follow while radar measurements are being acquired. All initially generated slices are shown in Figure 34, while Figure 35 shows only slices with radar detection.

[0111] Yet another example is provided in Figures 36 and 37 for the downtown area of ​​Calgary, California. As shown in Figure 36, during a first run, a position is determined using a navigation solution derived from motion sensor data and absolute navigation information. The position output of the integrated navigation solution can then be improved based at least in part on radar measurements. As described above, a nonlinear measurement model can be used for the radar measurements to integrate the motion sensor data and the radar measurements in a nonlinear state estimation technique. As can be seen, a map constructed using the improved navigation solution is more accurate, as shown in Figure 37.

[0112] It is contemplated that the present methods and systems may be used for any application involving integrating radar measurements with motion sensor data to provide a navigation solution. Without being limited to the above, the present disclosure is further illustrated by the following examples detailing illustrative implementations of integrating radar measurements with motion sensor data.

[0113] 1. Radar Measurement Model Implementation As mentioned above, nonlinear measurement models of radar measurements are used to directly update the nonlinear state estimation techniques used to provide an integrated navigation solution. As an example, information from a radar sensor adapted for use on a vehicle may have a data rate of 20 Hz. Each scan consists of a set of measurements covering the radar's FOV. Generally, a full scan is tagged as one measurement vector. A measurement vector is a set

[0114]

number

[0115]

number

[0116]

number

[0117]

number

[0118] Using the independence assumption between measurements, the probability of a scan is expressed as a multiplication between the probability of each individual measurement given knowledge of the map and vehicle state. These assumptions are used to simplify the modeling process. Four different radar measurement models using different modeling techniques are detailed below, although other models may be employed as desired.

[0119] 1.1 Range-Based Model Implementation One suitable radar measurement model is a range-based measurement model. Based on the true range between a static radar sensor and a static target, the model calculates a probability density function

[0120]

number

[0121]

number

[0122] 1.1.1 Ray casting implementation In range-based modeling, the true range to a target in a map (which may include a feature-based map, a position-based map, or both) may be calculated based on an estimate of the platform state. This is done by using a ray-casting algorithm to calculate the true range.

[0123]

number

[0124] To estimate the range between the vehicle and a target in the map, a ray (which does not need to propagate with respect to the RF path loss model) can be simulated to travel in a straight line until it either hits the target in the map or exceeds a certain distance (defined by the radar maximum range). The direction of the ray in 3D is based on the reported state of the platform (which may include position and orientation, also called "attitude") and the orientation of this particular measurement. A transformation between the radar coordinate frame and the vehicle coordinate frame can establish the starting point and direction of the ray relative to the state of the vehicle. Using this technique, the true range from the radar sensor to the target can be calculated.

[0125]

number

[0126] One of the main issues with ray-casting algorithms is the time complexity of the algorithm. To speed up the ray-casting algorithm, only ray-casting in a delta orientation around the measured value can be explored. This delta is used to account for errors in the radar sensor's Direction of Arrival (DOA) estimation. Therefore, instead of ray-casting virtual rays from all possible radar orientations, the exploration is limited to ray-casting around the orientation of the measured range. Furthermore, the coverage of the ray-casting algorithm is a function of the uncertainty in the range of the radar sensor (i.e., LRR, MRR, or SRR) and the estimated vehicle attitude. The higher the uncertainty, the larger the ray-casting coverage range. Limiting the number of ray-casting iterations (by limiting the ray-casting exploration within the orientation delta from the estimated DOA) and limiting the coverage of the ray-casting algorithm will help reduce the time complexity of the algorithm.

[0127] A schematic diagram of the architecture for estimating the probability of the platform's current state using a range-based radar model is shown in Figure 8. Here, particle state refers to the state of the vehicle at time t. The inputs to the system are radar range measurements and their respective azimuth and elevation angles. Given the platform state and map, ray casting is used to estimate the probability of p(ρ t │x t,m) can be estimated. Finally, Bel(x t To estimate the probability of the current state, represented by t and the probability of the measurement given the map, Bel(x t-1 ) multiplied by the prior probability of the previous state.

[0128] 1.1.2 Error Modeling Implementation When estimating the true range to a target using an initial estimate of the platform's state with a given map, various factors can affect radar sensor measurement errors. The measurement models of the present disclosure are configured to handle the errors, for example, by adapting to help compensate for these errors or otherwise intelligently addressing their presence, so that the measurement model can still provide good results and operate despite the errors. Sources of range error can be divided into three categories: a first source of error is environmental factors that affect radar measurements; a second source of error is inherent to the sensor itself; and a third source of error is related to the dynamics of the vehicle relative to the target.

[0129] Regardless of the source of the error, the measurement error due to a particular error source is, on average,

[0130]

number

[0131]

number

[0132]

number

[0133] The denominator is the probability density function p(ρ t │x t ,m), which is the normalization factor for the radar coverage range [ρ min ,ρ max ], the only missing parameter to be defined is σ err Leave.

[0134] Different sources of error have different effects on the variance of the measurement model. The three error sources identified above can be considered when building a probabilistic measurement model. The first source of error is environmental factors, including weather conditions such as rain, fog, and snow. Another important environmental factor is the effect of reflectors on the signal. A radar antenna emits a signal in a specific azimuth, and this signal is affected by the power delay profile (PDP) of the channel. Depending on the PDP, the reflected signal can be the sum of line-of-sight (LOS) to the target and non-LOS to the target. Due to multipath effects, the accuracy of radar measurements can be adversely affected. The second source of error is inherent in the design of the radar sensor itself. For example, in an FMCW-based radar, a VCO is used to generate the transmit signal by sweeping the available bandwidth. Ideally, this sweep should result in a linear relationship between frequency and sweep time. However, the measured target range is affected by variance error due to the nonlinearity of the FMCW radar sweep. This error is expressed as σ errThe error can be modeled using the standard deviation of the adaptive model of the present invention. Similarly, different radar technologies, such as pulse-based sensors, may have other inherent design issues that result in variance in range estimation and can be easily integrated into the model. The final source of error is related to the dynamics of the radar sensor relative to static or dynamic targets. Generally, these errors reflect the impact of target position, speed, and direction of motion on range estimation accuracy. For example, some radars have better range estimation accuracy for targets at 0 degrees and lower accuracy for targets at higher azimuth angles (relative to the radar center point). Furthermore, range estimation accuracy can also be affected by vehicle speed. Specifically, some radars have better range estimation at low Doppler frequency shifts between the radar and the target and lower range accuracy at high Doppler frequency shifts. Therefore, these errors can also be modeled using the standard deviation of the adaptive model of the present invention. It is also noteworthy that the target's speed relative to the radar sensor has the same effect on azimuth estimation.

[0135] Taking the above factors into consideration, one suitable Adaptive Measurement Model (AMM) is shown schematically in Figure 9. As shown, each block on the right represents three different factors that can affect the variance of the measurement model: environmental factors, radar design factors, and (platform) dynamic factors. For example, using FMCW with sweep nonlinearities that affect range estimation can lead to:

[0136]

number

[0137]

number

[0138]

number

[0139]

number

[0140]

number

[0141]

number

[0142] 1.1.3 Parameter Estimation Implementation In particular, building an AMM model may involve identifying the mean and variance of each error source. Once these parameters are estimated, an approximate PDF for the current measurements may be constructed. To do so, data may be collected according to the error source, using either domain expert intuition or designed experiments, and then an attempt may be made to find the best normal distribution that fits the collected data. Once the best-fit distribution is found, the mean and variance of the error source under investigation may be extracted. This mean and variance may then be stored for use when the same road conditions are applied.

[0143] 1.2 Implementation of Nearest Object Likelihood Model Another suitable radar measurement model is the nearest object likelihood (NOL) measurement model, which features reduced computational complexity compared to ray casting for each possible state used in range-based measurement models. Under this approach, the radar sensor position is first transformed from the local frame to a global frame.

[0144]

number

[0145]

number

[0146]

number

[0147]

number

[0148] The radar position in the global frame is (x rad,t ,y rad,t ,z rad,t), which can be correlated to a location in the map information. The next step is the current measurement

[0149]

number

[0150]

number

[0151]

number

[0152]

number

[0153]

number

[0154]

number

[0155] next,

[0156]

number

[0157]

number

[0158]

number

[0159]

number

[0160]

number

[0161] One suitable architecture for estimating platform state probabilities based on a radar NOL measurement model to estimate beliefs about the vehicle's current state is shown schematically in FIG. 10. Here, particle state refers to the vehicle's attitude at time t. The inputs to the system are radar range measurements and their respective azimuth and elevation angles. Given the vehicle's state and map, the vehicle's attitude is projected into 3D space for each measurement. Based on the distance to the nearest object from each projection, p(ρ t │x t ,m) is estimated. Bel(x t ) is the probability of the current state, denoted by t and the probability of the measurement given the map, Bel(x t-1) multiplied by the prior probability of the previous state. As will be appreciated, the error compensation techniques described above for range-based measurement models, such as those described in Section 1.1.2, can also be applied to NOL measurement models.

[0162] 1.3 Map-matching based model implementation Yet another suitable radar measurement model is a map matching model, which is characterized by the ability to account for objects not detected by the radar system by matching between a local map and a global map when assessing the likelihood of a scan given platform state and map knowledge. A local map is defined as a map created based on radar measurements. A global map may be either a feature-based map or a location-based map, as described above. As an example, a grid map of the environment containing the platform may be generated by the map matching model. gl and the measurement z t m loc The local map is assumed to be transformed into a local grid map represented by the rotation matrix

[0163]

number

[0164] Assuming a 3D map and 3D radar, the center of a grid cell on the global map is

[0165]

number

[0166]

number

[0167]

number

[0168]

number

[0169]

number

[0170]

number

[0171]

number

[0172] A schematic diagram of one possible architecture for a radar map matching measurement model that can be used to estimate beliefs about the current state of the platform is shown in Figure 11. Here, particle state refers to the state of the platform at time t. The inputs to the system are radar range measurements and their respective azimuth and elevation angles. Given the vehicle state and radar measurements, m loc A local map (e.g., a 2D or 3D occupancy grid) can be constructed, represented by m gl Using the same representation of the global map, denoted by p(ρ), the correlation between the global map and the local map is calculated. t │x t , m). Finally, Bel(x t To estimate the probability of the current state, represented by t and the probability of the measurement given the map, Bel(x t-1 ) multiplied by the prior probability of the previous state. It will be appreciated that the error compensation techniques described above for range-based measurement models, such as those described in Section 1.1.2, can also be applied to map-matching measurement models.

[0173] 1.4 Closed-form model implementation A further example of a radar measurement model that can be used in the techniques of the present disclosure is a stochastic closed-form model that relates radar measurement range to true range as a function of platform state, as compared to the probabilistic approach described above. A closed-form radar measurement model does not include a deterministic relationship between measured range and true range. To provide a relationship between platform state and measured range, a first correspondence / association is determined by assuming that an object detected by the radar is uniquely identified as an object in the map. Correspondence is provided by knowing which object is detected by the radar in the map given an estimate of the range to the object. There are several approaches to solving the correspondence problem. For example, if a map is represented as a feature map containing a set of objects, every object has a unique signature to the radar. Therefore, the detected object can be inferred by comparing the radar signature to the object signature. If they match, the object detected by the radar can be assumed to be the object that maximizes the correlation between the radar signature and the particular map object. If signatures from several objects result in the same (or close) correlation vector, the search area can be limited to a smaller cone centered on the radar's reported azimuth and elevation angles. Other approaches to solving the correspondence problem include using other visual sensors, such as cameras, to help classify the types of objects detected by the radar (i.e., speed limit road signs vs. traffic signs), thus limiting the search to objects of the same type and in the vicinity of the platform. As will be appreciated, the error compensation techniques described above for range-based measurement models, such as those described in Section 1.1.2, can also be applied to closed-form measurement models.

[0174] A single radar measurement,

[0175]

number

[0176]

number

[0177]

number

[0178]

number

[0179]

number

[0180]

number

[0181]

number

[0182]

number

[0183]

number

[0184] One embodiment of a closed-form radar measurement model is shown schematically in Figure 12. To detect and classify objects, it may be desirable to employ a 4D radar with high resolution in azimuth and elevation. Next, a first correspondence is made between the objects detected and classified by the radar sensor (labeled with range ranges) and the objects in the map. Resolving the radar / map correspondence leads to knowing the object's location in the global frame (absolute position). Correspondingly, a closed-form measurement model can be constructed using the objects' absolute positions and their ranges as a function of the platform state.

[0185] Another embodiment of a closed-form radar measurement model employing information from an auxiliary sensor is shown schematically in FIG. 13 . Suitable types of auxiliary sensors include optical cameras, thermal cameras, and infrared imaging sensors, which may be implemented as external sensors 116 or auxiliary sensors 114 of device 100, or in any other desired manner. Images or other samples from the auxiliary sensor may be used to detect and classify objects. A first correspondence is then determined by relating ranges from the radar to the classified objects. A second correspondence is then determined between the objects detected and classified by the auxiliary sensor (labeled with range ranges) and objects in the map. Resolving the camera / map correspondence leads to knowing the object's location in a global frame (absolute position). Thus, a closed-form measurement model may be constructed using the absolute locations of the objects and their ranges as a function of the platform state.

[0186] 2. Implementation of the System Model As mentioned above, another aspect of the techniques of this disclosure is the use of state estimation techniques to provide a navigation solution that integrates radar measurements with motion sensor data. The following material describes the use of an exemplary nonlinear system model and an alternative integrated navigation solution with an alternative state estimation technique. In one embodiment, a nonlinear error state model can be used to predict the error state, which can then be used to correct the actual state of the vehicle. Alternatively, in some embodiments, a linearized error state model can be used. In another embodiment, a nonlinear full-state model can be used to directly estimate the vehicle's state, including 3D position, velocity, and attitude angles. In yet another embodiment, INS and GNSS (or other source absolute navigation information) systems are integrated to provide a solution from an alternative state estimation technique (another filter) that is input into the system model for the state estimation technique at hand.

[0187] 2.1 Implementation of the nonlinear error state model In this embodiment, a three-dimensional (3D) navigation solution is provided by calculating the 3D position, velocity, and attitude of the mobile platform. The relative navigation information includes motion sensor data obtained from a MEMS-based inertial sensor consisting of three orthogonal accelerometers and three orthogonal gyroscopes, such as the sensor assembly 106 of the device 100 of FIG. 1. Similarly, the host processor 102 may implement an integration module 114 to integrate the information using a nonlinear state estimation technique, such as, for example, a mixed PF with a system model defined herein below.

[0188] Navigation Solutions The state of the device 100, whether tethered to a moving platform or untethered,

[0189]

number

[0190]

number

[0191]

number

[0192]

number

[0193] The motion model used for mechanization is x k =f mech (x k-1 ,u k-1 ) where u k-1 are the control inputs and inertial sensor readings corresponding to the transformation of state from time epoch k-1 to time epoch k, which is the convention used in this description of sensor readings for nomenclature purposes. The nonlinear error state system model (also called the state transition model) is k =f(δx k-1 ,u k-1 ,w k-1 ) where w k is the process noise that is independent of past and current states and accounts for uncertainties in the platform motion and control inputs. The measurement model is k =h(δx k ,v k ) where v k is the measurement noise, which is independent of past and present state and process noise and takes into account the uncertainty in the radar measurements.

[0194] In this and other examples, a common set of reference frames is used for demonstration purposes, and it should be recognized that other definitions of reference frames may be used. The platform's body frame has an X-axis along the vehicle's lateral direction, a Y-axis along the forward longitudinal direction, and a Z-axis along the vertical direction, completing a right-hand system. The local level frame is the ENU frame with axes along the east, north, and vertical (up) directions. The inertial frame is the Earth-centered inertial frame (ECI), which is centered on the Earth's center of mass and whose Z-axis is the Earth's axis of rotation. The Earth-centered Earth-fixed (ECEF) frame has the same origin and z-axis as the ECI frame, but rotates with the Earth (hence the term Earth-fixed).

[0195] A recursive mechanization process may be implemented to convert the inertial sensor outputs into position, velocity, and attitude information based on the previous outputs (or some initial values) from the inertial sensors and the new measurements. A suitable initialization procedure may then be implemented and tailored to the particular application. For example, initialization may be performed for position and velocity, such as by using the platform's last known position before it began moving, or may be provided by absolute navigation information, if available; velocity initialization may be performed with zero input if the platform is stationary, or, if moving, velocity may be provided from an external navigation source, such as GNSS or an odometer. When velocity is not available from absolute navigation information, the difference in position over time is used to approximate velocity, and the azimuth angle is calculated as a result. In some embodiments, the attitude angle may be calculated, such as by using a quaternion. Another suitable technique for calculating the attitude angle employs a skew-symmetric matrix of angle increments corresponding to a rotation vector from the local level frame to the body frame expressed in the body frame. Position and velocity may then be calculated accordingly. In general, it should be recognized that the mechanization equations for attitude, velocity, and position may be implemented differently, for example, using better numerical integration techniques for position. Additionally, coning and sculling may be used to provide more accurate mechanization outputs.

[0196] System Model As mentioned above, the system model is a state transition model, and since it is an error state system model, the system model transitions from the error state of the previous iteration k-1 to the error state of the current iteration k. To describe the system model utilized in the present navigation module, which is a nonlinear error state system model, the error state vector must first be described. The error state is the error of the navigation state, the rotation matrix that transforms from the device body frame to the local level frame,

[0197]

number

[0198]

number

[0199]

number

[0200]

number

[0201]

number

[0202]

number

[0203]

number

[0204] Sensor Error Modeling A system model for the sensor errors may be used. For example, a conventional model for these sensor errors in the literature is a first-order Gauss-Markov model, which can be used here, although other models can be used as well. For example, a higher-order autoregressive (AR) model can be used to model the drift in each of the inertial sensors. In general, if stochastic gyroscope drift is modeled in the system model by any model, such as Gauss-Markov (GM) or AR, the state vector must be expanded accordingly. A typical way to do this would be to add 120 states to the state vector for, say, an AR of order 120. This would incur significant computational overhead and require an increase in the number of particles used, so an alternative approach involves expanding the state vector in the PF by one state for the gyroscope drift, allowing for the use of a possible higher-order model without adding significant computational overhead. In some embodiments, errors in the rotation matrix that transforms from the device body frame to the local-level frame can be modeled. Additionally, attitude and velocity errors can be obtained. Finally, the position error can also be modeled.

[0205] 2.2 Implementation of a nonlinear full-state model In this embodiment, a three-dimensional (3D) navigation solution is provided by calculating the 3D position, velocity, and attitude of the mobile platform. The relative navigation information includes motion sensor data obtained from a MEMS-based inertial sensor consisting of three orthogonal accelerometers and three orthogonal gyroscopes, such as the sensor assembly 106 of the device 100 of FIG. 1. Similarly, the host processor 102 may implement an integration module 114 to integrate the information using a nonlinear state estimation technique, such as, for example, a mixed PF with a system model defined herein below.

[0206] Navigation Solutions The state of the device 100, whether tethered to a moving platform or untethered,

[0207]

number

[0208]

number

[0209]

number

[0210]

number

[0211] A nonlinear full-state system model (also called a state transition model) is x k =f(x k-1 ,u k-1 ,w k-1 ) where u k-1 is the control input and is the inertial sensor reading corresponding to the transformation of state from time epoch k−1 to time epoch k, which is a convention used in this description of sensor readings that is used for nomenclature purposes only. Furthermore, W k is the process noise that is independent of past and current states and accounts for uncertainties in the platform motion and control inputs. The measurement model is k =h(z k ,v k ) where v k is the measurement noise, which is independent of past and present state and process noise and takes into account the uncertainty in the radar measurements.

[0212] 2.2.1 System Model The system model is a state transition model, and since it is a full-state system model, the system model transitions from all states in the previous iteration k-1 to all states in the current iteration k. Before describing the system model utilized in this example, the control inputs are first introduced. The measurements provided by the IMU are used as the control inputs

[0213]

number

[0214]

number

[0215]

number

[0216] To describe the system model utilized in this example, which is a nonlinear full-state system model, the full state vector must first be described. The state consists of the navigation state itself and the errors in the sensor readings (i.e., the errors in the control inputs). The navigation state is

[0217]

number

[0218]

number

[0219]

number

[0220]

number

[0221]

number

[0222] Sensor Error Modeling Similar approaches to modeling sensor errors as described for the nonlinear error state model embodiment described in Section 2.1 may be used here. It should be recognized that the system model equations for attitude, velocity, and position may be implemented differently, for example, using better numerical integration techniques for position. Additionally, coning and sculling may be used to provide more accurate navigation state outputs.

[0223] 2.3 System Model with Alternative Integrated Filters As mentioned above, other embodiments integrate the INS and GNSS systems to use a system model that utilizes solutions from other state estimation techniques (other filters) that are input into the system model for the state estimation technique at hand. For example, a Kalman filter-based navigation solution (among other state estimation techniques) can be used as input to the system model for the current state estimation technique. This solution can integrate inertial sensor data with GNSS updates using a Kalman filter solution (one example among other state estimation techniques). Other sources of absolute updates that can be integrated into the solution include an odometer for speed updates, a magnetometer for heading updates, and a barometer for altitude updates. One suitable architecture is shown schematically in FIG. 14, which shows a basic block diagram of a Kalman filter-based positioning solution that uses absolute information (obtained from various sensors) to estimate inertial sensor errors and subtract them from the INS solution to obtain a final corrected solution.

[0224] A solution from a Kalman filter-based system may include an estimate of the system state and the uncertainty of the solution. The following equation shows an example of a system model for the current state estimation technique based on another integrated solution that uses another state estimation technique (in this example, a Kalman filter-based solution): φk =φ k-1 +(φ k,sol -φ k-1,sol )+φ noise λ k =λ k-1 +(λ k,sol -λ k-1,sol )+λ noise h k =h k-1 +(h k,sol -h k-1,sol )+h noise where φ k ,φ k-1 ,λ k ,λ k-1 ,h k ,h k-1 are the current and previous latitude, longitude, and altitude of the system model, respectively.

[0225] φ k,sol , φ k-1,sol , λ k,sol , λ k-1,sol , h k,sol , h k-1,sol are the current and previous latitude, longitude, and altitude of the Kalman filter-based solution, respectively. Finally, φ noise , λ noise , h noise The random variables in represent the process noise added with the following distribution:

[0226]

number

[0227] 3-state estimation Next, details regarding the design of a state estimator for integrating radar data and motion sensor data are described. As mentioned above, four exemplary radar measurement models include a range-based model, a NOL model, a radar map matching model, and a closed-form model. Each can be integrated with the system model described immediately above. For purposes of this disclosure, the integration of each radar measurement model is in the context of a particle filter (PF) state estimator. PF estimators can be used when the system and / or measurement model is nonlinear, as opposed to other state estimation filters such as a Kalman filter (KF), which require linearization. Furthermore, PF estimators are more suitable when the noise affecting the measurement model or system model is non-Gaussian. In other words, PF can be used to represent multimodal error distributions. Furthermore, PF estimators provide a multi-hypothesis approach, while KFs propagate a single hypothesis.

[0228] However, other nonlinear state estimation techniques are also within the scope of this disclosure. For example, another filtering technique that can be used is a mixed PF. We first describe some aspects of a basic PF called Sampling / Importance Resampling (SIR) PF. In the prediction phase, the SIR PF samples from a system model that does not depend on the last observation. In MEMS-based INS / radar integration, sampling based on a system model that relies on inertial sensor readings as control inputs degrades the performance of the SIR PF because, with more drift, this sampling operation does not generate enough samples in regions where the true probability density function (PDF) of the state is large, especially in the case of MEMS-based sensors. Due to the limitations of the SIR PF, a very large number of samples must be used to ensure good coverage of the state space, which is therefore computationally expensive. The mixed PF is a variant of PF that aims to overcome this limitation of SIR and use a much smaller number of samples without sacrificing performance. Due to the much smaller number of samples, the mixed PF can be applied in real time.

[0229] As mentioned above, in SIR PF, samples are predicted from the system model, and then the most recent observations are used to adjust the importance weight of this prediction. Hybrid PF adds some samples predicted from the most recent observations to the samples predicted from the system model. The importance weights of these new samples are adjusted according to the probability they derive from the samples of the last iteration and the most recent control input.

[0230] During the sampling phase of the hybrid PF used in this embodiment, some samples predicted according to the latest observations are added to samples predicted according to the system model. The latest observations are used to adjust the importance weights of the samples predicted according to the system model. The importance weights of the additional samples predicted according to the latest observations are adjusted according to the probabilities generated from the system model with the samples of the last iteration and the latest control input. When no radar object is detected, only samples based on the system model are used, but when an object is detected, both types of samples are used to provide better performance, especially during areas without radar detection. Additionally, adding samples from radar observations results in faster recovery to the true position after a radar outage (a period during which no object is detected by the radar).

[0231] It is also worth noting that a KF filter can be used if the sensor model and the system model are linear. If either the sensor model or the system model are not linear, a different form of KF, such as an extended Kalman filter (EKF), can be used to linearize the model before running the filter.

[0232] 3.1 Measurement Model: Range-Based Implementation This explanation is based on the probability density function

[0233]

number

[0234]

number

[0235]

number

[0236]

number

[0237] Each measurement value

[0238]

number

[0239]

number

[0240]

number

[0241]

number

[0242]

number

[0243]

number

[0244] The radar measurements are then calculated for each of the N particles as p(ρ t │x t ,m) for each detected object using a ray casting algorithm.

[0245]

number

[0246] 3.2 Measurement Model: Nearest Object Likelihood Implementation This example shows that p(ρ t │x t The aim of this paper is to integrate the state model with a nearest object likelihood model, which does not require ray-casting operations to calculate (k, m). The first step is to filter out all measurements reflected from moving objects (since the map can only contain static objects). Next, measurements for static targets are projected onto the map in the global frame. For example, the projected radar position from the kth measurement is given by the following equation, where:

[0247]

number

[0248]

number

[0249]

number

[0250]

number

[0251]

number

[0252]

number

[0253]

number

[0254] p(ρ t │x t The probability of scanning measurements, represented by (m, m), can be directly used to weight the importance of particles with known states in the map in this integration of the NOL measurement model with the MEMS-based full-state system model. In the context of a basic PF or sampling / importance resampling (SIR) filter, the PF is initialized by generating N particles using a random distribution (which can be within a limited distance from the initial state). In the prediction stage, the system model is used to propagate the states of the N particles based on inputs from the inertial sensors and the proposed system model. The state of each particle is represented by the vector

[0255]

number

[0256]

number

[0257]

number

[0258]

number

[0259] The radar measurements are then used to project the state of the platform into a map, and the distance to the nearest object is then used as input to a measurement model to calculate p(ρ t │x t,m). Before the projection step, all moving objects / targets are filtered by utilizing relative Doppler information. As a result, an importance weight is associated with each particle proportional to the proximity of the projected state to the nearest object in the map. A resampling step is then required to randomly select N new particles from the N old particles in proportion to the normalized importance weight of each old particle (typically, these weights are normalized by dividing each particle's weight by the sum of all weights). Therefore, particles with low importance weights have a high probability of not propagating to the next state. In other words, surviving particles usually cluster around areas with higher posterior probabilities. A resampling step is then required to randomly select N new particles from the N old particles in proportion to the normalized importance weight of each old particle (typically, these weights are normalized by dividing each particle's weight by the sum of all weights). Therefore, particles with low importance weights have a high probability of not propagating to the next state. In other words, surviving particles usually cluster around areas with higher posterior probabilities.

[0260] As mentioned above, the basic SIR PF filter has some limitations because samples are predicted solely from the system model, and then the most recent observations are used to adjust the importance weight of this prediction. A mixed PF adds additional samples predicted from the most recent observations to the samples predicted from the system model. The importance weights of these new samples are adjusted according to the probability that they were derived from the samples of the last iteration and the most recent control input. In the context of a NOL-based radar measurement model, a preferred method generates particles extracted from the measurement model. This can be done by searching for a state where the object list detected by the radar is closely aligned with objects in the global map (i.e., a high probability of radar measurements given the current state). A measure of how well the object list is aligned given a particular state is calculated using the NOL radar measurement model as p(ρ t │x t,m) is calculated. If such a state is found, it is possible to generate particles extracted from the measurement model rather than the system model. To make this process efficient, we should consider the search space (an infinite number of states) in the global map, thereby allowing us to effectively apply constraints to limit the search space and consequently reduce the computational complexity. After new particles are successfully extracted from the NOL-based measurement model, the importance weights of these new samples are adjusted according to the probability they obtained from the samples of the last iteration and the latest control inputs.

[0261] 3.3 Measurement Model: Radar Map Matching Implementation In this embodiment, a radar map matching measurement model is integrated with the system model. As described above, the map matching model is based on applying a matching algorithm between a local map (radar scan) and a global map (e.g., a reference for radar measurements) as a means of measuring the likelihood of a scan given state and map knowledge. A local map is defined as a map created based on radar measurements. A global map may be either a feature-based map or a location-based map. Regardless of the type of map used, the map can be converted to an appropriate format (OGM map) so that the map can be directly matched to the radar scan.

[0262] Assuming a 3D radar map and 3D radar, the center of a grid cell in the global map is

[0263]

number

[0264]

number

[0265]

number

[0266]

number

[0267]

number

[0268]

number

[0269] The correlation coefficient ranges from +1 to -1, but is only significant if there are only positive correlations or no correlations at all, so we can assume that all negative correlations are equal to 0, and we can calculate the likelihood of a radar scan as

[0270]

number

[0271] p(ρ t │x tThe probability of scanning measurements, represented by (m, m), can be directly used to weight the importance of particles with known states in the map. Here, we describe the integration of the radar map matching measurement model with a MEMS-based full-state system model. In the context of a basic PF or sampling / importance resampling (SIR) filter, the PF is initialized by generating N particles using a random distribution (which can be within a limited distance from the initial state). In the prediction stage, the system model is used to propagate the states of the N particles based on inputs from inertial sensors and the proposed system model. The state of each particle is represented by the vector

[0272]

number

[0273]

number

[0274]

number

[0275]

number

[0276] The radar measurements are then used to create a local radar map, which is then iteratively matched with the global map until the current scan correlates given the particle's state. The calculated correlations are then used to calculate p(ρ t │x t ,m). Before the map matching step, all moving objects / targets are filtered by utilizing relative Doppler information. As a result, an importance weight is associated with each particle proportional to the correlation between the local map and the global map given the particle's state. A resampling step is then required to randomly sample N new particles from the N old particles in proportion to the normalized importance weight of each old particle (typically, these weights are normalized by dividing each particle's weight by the sum of all weights). Therefore, particles with low importance weights have a high probability of not propagating to the next state. In other words, surviving particles usually cluster around areas with higher posterior probabilities.

[0277] Again, the basic SIR PF filter has some limitations, since samples are predicted solely from the system model, and then the most recent observations are used to adjust the importance weight of this prediction. Therefore, in this example, the use of a hybrid PF allows for the addition of additional samples predicted from the most recent observations, in addition to the samples predicted from the system model. The importance weights of these new samples are adjusted according to the probability that they were derived from the samples of the last iteration and the most recent control inputs. In the context of a radar map-matching measurement model, a method for generating particles extracted from the measurement model can be employed. This can be done by searching for a state where measurements from the radar are closely aligned with measurements from the global map at a particular location (i.e., a high probability of the radar measurement given the current state). A measure of how well the radar measurements are aligned given a particular state can be calculated using the map-matching correlation coefficient, p(ρ t │x t,m). In other words, by matching the local map to the global map in different states and saving the states where the match between the local map and the global map results in a high correlation coefficient, some matches can be found. If such a state is found, particles extracted from the measurement model rather than the system model can be generated. To make this process efficient, we should consider the search space (an infinite number of states) in the global map and effectively apply constraints that limit the search space and consequently reduce the computational complexity. After new particles are successfully extracted based on the map-matching-based measurement model, the importance weights of these new samples are adjusted according to the probability they were obtained from the samples of the last iteration and the latest control input.

[0278] 3.4 Measurement Model: Closed-Form Implementation This section describes the integration of the map matching model with the system model and includes specific, non-limiting examples. As mentioned above, map matching is a non-probabilistic modeling approach that assumes that objects detected by the radar can be uniquely identified as objects in the map. Given an estimate of the range to the object (which may be supplemented with relative Doppler information) and knowledge of which objects in the map were detected by the radar, correspondences can be determined that relate measurements to the state of the platform in a closed-form model.

[0279] In this tightly coupled mixed PF integration, radar raw data is used and integrated with inertial sensors. The radar raw data used in this navigation module in this example is range and Doppler shift. From the measured Doppler for each target, the corresponding range rate can be calculated. In the update phase of the integration filter, the range and range rate can be used as measurement updates to update the position and velocity states of the vehicle. The measurement model relating these measurements to the position and velocity states is a nonlinear model.

[0280] As is known, the KF integration solution linearizes this model. PF, with its ability to handle nonlinear models, may offer improved performance for tightly coupled integration due to the fact that the system model may be nonlinear, as well as the ability to use accurate nonlinear measurement models. The arrival frequency at the radar receiver is not the exact frequency of the signal reflected by the target, but is shifted from the original value transmitted by the radar. This is called the Doppler shift and is due to the relative motion between the object / target and the radar receiver. The Doppler shift from the mth object is the projection of the relative velocity (between the object and the receiver) onto the line-of-sight vector multiplied by the transmission frequency and divided by the speed of light,

[0281]

number

[0282]

number

[0283]

number

[0284]

number

[0285]

number

[0286]

number

[0287] Conventional techniques relying on KFs are used to linearize these equations for range estimates obtained from inertial sensor mechanization. In this example, a PF is proposed to accommodate the nonlinear model, so there is no need to linearize this equation. A suitable nonlinear radar range model for M detected objects is:

[0288]

number

[0289] The position state x in the above equation is in ECEF Cartesian coordinates, which can be converted to geodetic coordinates for the state vector used in the mixed PF. The relationship between geodetic and Cartesian coordinates is:

[0290]

number

[0291]

number

[0292] The true range rate between the mth object and the radar sensor is

[0293]

number

[0294]

number

[0295]

number

[0296] It will be appreciated that the above equation is linear for velocity but nonlinear for position. This can be seen by examining the expression for the line-of-sight unit vector above. Again, no linearization is necessary due to the nonlinear capabilities of the PF. Again, the nonlinear model for range-rate of M targets in ECEF Cartesian coordinates is:

[0297]

number

[0298]

number

[0299]

number

[0300]

number

[0301] These concepts are now illustrated in the following non-limiting examples.

[0302] 3.4.1 Example 1: Measurement Model for Error State System Model As explained, the measurement model is the difference δz between the mechanized estimates of range and range rate at time epoch k and the raw radar measurements (range measurements and range rate). k Let δx be the state at time k. k and measurement noise ε k First, the raw radar measurements are given by

[0303]

number

[0304]

number

[0305]

number

[0306]

number

[0307]

number

[0308]

number

[0309]

number

[0310]

number

[0311]

number

[0312]

number

[0313]

number

[0314] 3.4.2 Example 2: (1) Full-State System Model, (2) Measurement Model for System Model with Separate Integrated Filter In the following example, the measurement model of the current nonlinear state estimation technique, whether it is a full-state system model or a system model based on another integrated filter or state estimation technique, is a function of the radar raw measurements (range measurements and range rate) z at time epoch k. k Let x be the state at time k. k , and measurement noise ε k This also applies to system models based on other integrated filter solutions. First, the radar raw measurements are

[0315]

number

[0316]

number

[0317]

number

[0318]

number

[0319]

number

[0320]

number

[0321]

number

[0322]

number

[0323]

number

[0324] 4 Radar-based updates 4.1 Radar Doppler Shift Update In a further aspect, Doppler shift may be used as an update. It will be recognized that one of the primary observables from radar is the Doppler information associated with each target. This raw data is independent of the radar range estimate. The arrival frequency at the radar receiver is not the exact frequency of the signal reflected by the target, but has been shifted from the original value transmitted by the radar. This is called Doppler shift and is due to the relative motion between the object / target and the radar receiver. The Doppler shift from the mth object is the projection of the relative velocity (between object and receiver) onto the line of sight vector multiplied by the transmitted frequency and divided by the speed of light,

[0325]

number

[0326]

number

[0327]

number

[0328] D m is directly observable, and V m and f tr(the frequency of the transmitted signal) is known, the only unknown is the velocity V. The part of the measurement model for range rate is

[0329]

number

[0330]

number

[0331]

number

[0332]

number

[0333] This absolute update from the radar can be used in conjunction with a probabilistic measurement model, i.e., a range-based model, a nearest object likelihood model, or a radar map matching model, to influence the importance weight of the particles.

[0334] 4.2 Radar / Map-based Positioning The above description of state estimation involves integrating radar observables (range and Doppler shift) with MEMS-based sensors using a tightly coupled approach for state estimation. In a further aspect, radar scans and maps can be used to estimate the platform state in a global frame at any given time and then integrate the radar-estimated state with MEMS-based sensors using a loosely coupled integration approach. The maps can include feature-based maps, position-based maps, or both. The vehicle's position can be estimated by matching the radar's current scan with a surveyed database of scans, with each scan associated with a state. The scan that results in the highest match indicator (e.g., correlation coefficient) can be used to infer the vehicle's state as estimated by radar / map integration. Another approach is to use unique features in the map that can be detected by the radar, which, once detected, can infer location. For example, if the radar detects a very specific distribution of road signs across its field of view, an equivalent geometric distribution of the signs can be searched for in the map, thereby inferring location based on radar map matching, previous platform position estimates, and other constraints. Motion constraints, such as nonholonomic constraints, can be applied to limit the search space for matches in the map.

[0335] These loosely coupled approaches can be employed with error-state or full-state system models. In the case of error-state system models, loosely coupled integration uses position and velocity updates from the radar / map estimator. Thus, measurements are

[0336]

number

[0337]

number

[0338]

number

[0339] For the full-state system model, the loosely coupled integration uses position and velocity updates from the radar / map estimator. Therefore, the measurements are

[0340]

number

[0341]

number

[0342] 4.3 Radar-based relative updates (object flow analysis) In one aspect, radar measurements can be used to estimate the platform's relative update (or attitude) by performing flow analysis on the radar measurements. This update can be fused with inertial and GNSS measurements to estimate a more accurate state. One suitable architecture is shown schematically in FIG. 15. The first step in this system is a block that filters out all moving objects from the radar frame. A radar frame is defined as a full radar scan of the field of view. The filtered radar frame consists of the centroids of stationary objects detected by the radar. The second step is to link the same objects between frames based on different measurement characteristics, such as the radar cross section (RCS) and reflected power of each object in the frame.

[0343] The second block is fr t-1 Compared to the object in the previous frame, represented by t The object flow tracker step tracks the object in the current frame, represented by fr t-1 exists in fr t The association / matching step tags objects that are not present in the frame as objects that are leaving the frame. t exists in the frame of fr t-1 Objects not present in fr should be tagged as incoming objects in the current frame and can be used to help estimate the next platform state. t and fr t-1 may be tagged as relevant for the relative update estimation at time t.

[0344] After solving the association problem between objects in the previous and current frames, the next step is to calculate the magnitude and direction of movement of static objects in the current frame relative to the previous frame. The relative pose estimation step estimates the relative update between two frames (i.e., the change in vehicle pose within time [t, t-1]) based on information about the movement of static objects in the current frame relative to the previous frame.

[0345] 4.4 Radar Doppler-based relative updates Doppler information about static objects can be used to estimate the relative update between two radar frames. A radar frame is defined as a full Doppler scan across the radar's field of view. The first step is to filter all moving targets (such as vehicles and pedestrians) using the Doppler information and the vehicle's odometer speed. The output of the first step is a frame containing all static detected objects, with each object's associated Doppler relative to the host vehicle.

[0346] The next step utilizes the relative Doppler of static objects in the current frame to estimate the change in the platform state. Thus, the relative pose estimation step estimates the relative update between two frames (i.e., the change in the vehicle pose within time [t, t-1]) based on information about the relative Doppler of static objects in the current frame.

[0347] 5. Inconsistency detection and estimation In yet another aspect, the techniques of the present disclosure can be applied to detect and determine misalignment. As mentioned above, misalignment may refer to either mounting misalignment when the device is strapped to the platform or fluctuating misalignment when the device is not strapped. Therefore, an optional misalignment procedure can be implemented to calculate the relative orientation between the frame of the sensor assembly (i.e., the device frame) and the frame of the moving platform. The following description includes four specific, non-limiting examples.

[0348] The device's heading, pitch, and roll (device attitude angles) can differ from the platform's heading, pitch, and roll (platform attitude angles), and to accurately derive a navigation solution for the platform and / or device (processed on the device), the navigation algorithm should have information about the misalignment as well as the platform's absolute attitude. This misalignment detection and estimation is intended to enhance the navigation solution. To improve navigation by applying constraints to the motion of a moving platform (e.g., in the form of specific updates), the platform attitude angle must be known. Since the device attitude angle is known, the misalignment angle between the device frame and the platform frame is needed to derive the platform attitude angle. If the misalignment angle is known, the following constraints are examples of those that can be implemented to constrain the navigation solution, especially during long absolute velocity outages (such as GNSS signal outages). Example uses include nonholonomic constraints, vehicle dead reckoning, and any other position or velocity constraints that may be applied to the platform after attitude solution.

[0349] Example 1: Heading Mismatch Using Absolute Velocity Updates In a first example, absolute velocity updates are used to estimate the heading misalignment. To calculate the portable device's heading from the gyroscope, the device's initial heading must be known. If an absolute velocity source (such as from GNSS) is not available (e.g., due to an interruption) but the magnetometer is available and has a suitable reading, it is used to obtain the initial device heading. If an absolute velocity source is available and the magnetometer is not available or does not have a suitable reading, the velocity source can be used to obtain the mobile platform's initial heading, a routine can be performed to obtain the portable device's initial heading misalignment relative to the mobile platform (described below), and then the initial device heading. If an absolute velocity source is available and the magnetometer is available and has a suitable reading, a blended version of the initial device heading calculated from the above two options can be formed.

[0350] This example details a suitable routine for obtaining an initial heading misalignment of a portable device relative to a moving platform when an absolute velocity source (such as GNSS) is available. This routine requires (i) an initial heading of the platform (person or vehicle or other) that can be obtained from a source of absolute velocity provided the device is not stationary, and (ii) a source of absolute velocity that is available for a short duration, e.g., about 5 seconds.

[0351] The procedure of this routine is to use the absolute velocity in the local level frame to generate the acceleration in the local level frame, add the gravitational acceleration from the gravity model, and then use the pitch and roll together with the differential heading values ​​(device heading corrected for the different heading misalignment values) to calculate the acceleration (more precisely, specific forces) in the estimated sensor frame. First, different heading misalignments are selected to cover all 360° of uncertainty. The actual accelerometer readings are corrected for sensor errors (such as bias, scale factor, and non-orthogonality) and then compared to all the differential calculations (an example of a technique that can be used here is a correlation technique). The best sector of possible heading misalignments is selected, and more candidates for heading misalignment in this sector are divided. Different accelerations in the estimated sensor frame are generated and again compared with the actual sensor readings. This operation continues until either the accuracy of the solution saturates and no longer improves, or until a preselected comparison depth is received.

[0352] As mentioned above, if an absolute velocity source (such as from GNSS) is not available (e.g., due to an interruption) but the magnetometer is available and has a suitable reading, it is used to derive the initial device heading. If an absolute velocity source is available and the magnetometer is not available or does not have a suitable reading, the velocity source is used to determine the initial heading of the mobile platform when it begins to move.

[0353]

number

[0354]

number

[0355] The routine requires tracking of the heading of the platform (vehicle or otherwise) for a short period of time (e.g., about 5 seconds). However, during this period, the platform cannot be stationary for the entire period, but there are few constraints on the platform's movement, except that temporary periods of stationaryness are tolerated. This heading can be obtained by one of the following: i) A primary heading for the platform that can be derived from a source of absolute velocity provided the platform is not stationary, and this heading is tracked by (for example) a gyroscope-based heading calculation to maintain tracking of the platform heading when the device misalignment relative to the platform is kept approximately constant (may vary slightly, but not by much). ii) Tracking of the platform's absolute heading can be obtained from an absolute velocity source during the short period this routine is executed. If the platform stops temporarily during this period, the last heading is used for this temporary stop period.

[0356] The routine also requires that a source of absolute rate be available for the same short period of time mentioned above, whatever data rate this source provides.

[0357]

number

[0358] The first step in this routine is to use the absolute velocity in the local level frame to generate the acceleration in the local level frame.

[0359]

number

[0360] where Δt is the sampling rate of the absolute velocity source. The next step is to add the gravitational acceleration from the gravity model to obtain a specific force in the local level frame.

[0361]

number

[0362]

number

[0363]

number

[0364]

number

[0365] The rotation matrix for transformation from the device frame (i.e., the accelerometer frame) to the local level (ENU) frame may be used as follows:

[0366]

number

[0367]

number

[0368]

number

[0369] For example, the best sector is

[0370]

number

[0371]

number

[0372]

number

[0373] The aforementioned operations are then repeated. Different candidate accelerations (or more precisely, specific forces) in the estimated sensor frame are generated and again compared to the downsampled actual sensor readings. This operation continues until either the accuracy of the solution saturates and no longer improves, or until a certain preselected depth of comparison is achieved. The estimate of the mismatch between the portable device heading and the platform heading is calculated based on the best ΔA along with an indication or measure of its accuracy from the depth of division the technique has gone through and the step separation of the final candidate pool for mismatch. candidate Thus, the initial device heading (which in this case is used to start the full navigation) is calculated from the platform heading and the estimated initial misalignment.

[0374] Example 2: Heading misalignment using turning radius In a next example, using motion sensor data in the presence or absence of absolute navigation information updates, misalignment between the device and the platform can be determined from the turning radius of the device. Details regarding suitable techniques can be found in commonly owned U.S. patent application Ser. No. 14 / 917,730, filed March 9, 2016, which is incorporated herein by reference in its entirety.

[0375] Example 3: Heading Misalignment Using Acceleration and Deceleration In another example, motion sensor data, in the presence or absence of absolute navigation information updates, may be utilized to determine misalignment between the device and the platform from the acceleration and / or deceleration of the platform. Details regarding suitable techniques may be found in commonly owned U.S. Patent No. 9,797,727, issued October 24, 2017, which is incorporated herein by reference in its entirety.

[0376] Example 4: Pitch Mismatch Using Absolute Velocity Updates In a final exemplary embodiment, absolute velocity updates can be used to estimate pitch misalignment. The device pitch angle can differ from the platform pitch angle due to mounting misalignment or varying misalignment when the device is not strapped on. To enhance the navigation solution, the pitch misalignment angle is calculated. By definition, the pitch misalignment angle is the difference between the device pitch angle and the platform pitch angle. To calculate the pitch misalignment angle, a state estimation technique is used. One possible example of a system model that can be used is a Gauss-Markov process, where measurements are taken from GNSS velocity and accelerometer measurements and applied as measurement updates via a measurement model. One suitable technique employs the following equation, where the measurement is the difference between the system pitch angle and the GNSS pitch angle:

[0377]

number

[0378] 6 other optional observations Yet further aspects of the present disclosure relate to using other observables, including information from GNSS positioning systems and odometers, as measurement updates in state estimation techniques when integrating radar measurements and motion sensor data. These optional observables can be used to estimate more accurate state.

[0379] Three main types of INS / GNSS integration have been proposed to achieve maximum benefit depending on the type of use and the preference for simplicity versus robustness, resulting in three main integration architectures: loosely coupled, tightly coupled, and ultra-tightly (or deep) coupled. Loosely coupled integration uses estimation techniques to integrate inertial sensor data with the position and velocity outputs of the GNSS receiver. The distinguishing feature of this configuration is a separate filter for GNSS, and it is an example of cascade integration because two filters (the GNSS filter and the integration filter) are used in succession. Tightly coupled integration uses estimation techniques to integrate inertial sensor readings with raw GNSS data (i.e., pseudoranges, which can be generated from code or carrier phase or a combination of both, and pseudorange rates, which can be calculated from Doppler shifts) to obtain vehicle position, velocity, and orientation. In this solution, there is no separate filter for GNSS, but rather a single common master filter that performs the integration. The loosely coupled integration method requires at least four satellites to provide acceptable GNSS position and velocity inputs to the integration technique. An advantage of the tightly coupled approach is that fewer than four satellites can be used, as this integration can provide GNSS updates even when fewer than four satellites are visible, which is typical of actual orbits in urban environments and dense forest canopies and steep hills. Another advantage of tightly coupled integration is that satellites with insufficient GNSS measurements can be detected and excluded from being used in the integrated solution. Ultra-dense (deep) integration has two main differences compared to other architectures. First, there is a fundamental difference in the architecture of the GNSS receiver compared to those used in loose and tight integration. Second, information from the INS is used as an integral part of the GNSS receiver; therefore, the INS and GNSS are no longer independent navigators; the GNSS receiver itself accepts feedback. It should be understood that a navigation solution can be utilized in any of the aforementioned types of integration.

[0380] It should be noted that the state estimation or filtering techniques used for inertial sensor / GNSS integration can function in either the full-state approach or the error-state approach, each of which has the properties described above. It is known to those skilled in the art that not all state estimation or filtering techniques can function in both approaches.

[0381] To help explain these above concepts, examples of a first error state system model and a full state system model that integrate absolute navigation information with the error state system model are described below. In these examples, a three-dimensional (3D) navigation solution is provided by calculating the 3D position, velocity, and attitude of the mobile platform. The relative navigation information includes motion sensor data obtained from a MEMS-based inertial sensor consisting of three orthogonal accelerometers and three orthogonal gyroscopes, such as the sensor assembly 106 of the device 100 of FIG. 1. Sources of absolute navigation information 116 are also used, and the host processor 102 may implement an integration module 114 to integrate the information using a nonlinear state estimation technique, such as mixed PF. Reference-based absolute navigation information 116, such as from a GNSS receiver, and motion sensor data, such as from the sensor assembly 102, are integrated using a mixed filter in either a loosely coupled, tightly coupled, or hybrid loosely / tightly coupled architecture with a system and measurement model. The system model is either a nonlinear error-state system model or a nonlinear full-state model without linearization or approximation, used in conjunction with traditional KF-based solutions and their linearized error-state system models. The filter can optionally be programmed to include advanced modeling of stochastic drift of the inertial sensors. If the filter has the latter option, the filter can optionally be further programmed to use derived updates for such drift from the GNSS, if necessary. The filter can optionally be programmed to automatically detect and evaluate the quality of the GNSS information and further provide a means to discard or ignore degraded information. The filter can optionally be programmed to automatically select between a loosely coupled and a tightly coupled integration scheme. Furthermore, if a tightly coupled architecture is selected, GNSS information from each available satellite can be independently evaluated and either discarded (if degraded) or utilized as a measurement update. In these embodiments, the navigation solution for the device, whether tethered to a moving platform or not,

[0382]

number

[0383]

number

[0384]

number

[0385]

number

[0386] Correspondingly, the techniques of this disclosure may involve integration of GNSS with error state system models or full state system models, as appropriate. As will be appreciated, architectures in which the measurement models are loosely or tightly coupled, as well as hybrid loosely / tightly coupled schemes may be used.

[0387] 7 Additional optional modules Yet further aspects of the present disclosure relate to optional operations or modules that can be utilized in conjunction with the techniques described above. The following sections provide non-limiting examples of some of these options.

[0388] 7.1 Radar-based simultaneous localization and mapping Furthermore, radar-based simultaneous localization and mapping (SLAM) processes can be incorporated into the techniques of the present disclosure. As can be appreciated, SLAM involves determining position information about a platform while building a map. Since no map is provided, it is assumed that the absolute positions of objects are unknown. Therefore, the map, including the platform state and object list, is unknown. A practical solution to this problem is to utilize a graph SLAM algorithm to estimate the platform state and build the map as a list of landmarks. In graph-based SLAM, nodes in the graph represent different states of the platform, and links between the nodes indicate constraints set by motion sensors. Furthermore, landmarks are also represented as nodes, and links between landmarks and platform state nodes at time t represent distance constraints. SLAM equations are solved by finding the optimal states (i.e., positions, poses) of the nodes such that all constraints are satisfied. There are also several techniques utilized by graph SLAM to incorporate uncertainties in the motion model and measurement model into the SLAM equations. It is also assumed that the initial state of the platform is known. Furthermore, online graph SLAM can be used to reduce the complexity of the graph by removing old states and keeping only one node to represent the current state of the vehicle.

[0389] Radar scanning can be used in conjunction with an object detection module to estimate the distance to the center of gravity of objects within the radar's field of view. Other characteristics of the reflected RF signal at a specific angle, such as the signal's power, radar cross section (RCS), relative Doppler frequency, etc., can be used to track objects and provide unique identifiers for static objects. It may also be desirable to use the relative Doppler information detected by the radar to filter moving objects. Motion can be used to predict the vehicle's next state; therefore, the Euclidean distance between the current and previous states can be used to link both nodes in the graph. Uncertainty in the motion model is propagated to the Euclidean distance calculation and can be used to adjust the importance of links between poses in the graph. Furthermore, uncertainty in the measurement model is propagated to the distance between landmarks and the current state and can be used to modulate the importance of links between the current state and detected landmarks.

[0390] 7.2 Radar signature map enhancement using reflected power In yet another aspect, a radar signature map can be updated with reflected power information. Radar measurements can be used to construct an occupancy grid or 3D model of the world (i.e., a global map) using a mapping vehicle or other platform. The measurements are associated with the platform's precise location. This map can then be used for localization purposes, for example, by finding the best match between the local map created by the radar and the global map. The best match is associated with a location, and this location is inferred to be the platform's current location. Often, multiple good matches can be found that do not distinguish the best match (e.g., if the correlation indicators between the global and local maps are very close for some of the matches).

[0391] To solve the problem of multiple matches, the reflected power at each measurement from the radar sensor can be recorded during the mapping process. In other words, when the map is constructed, in addition to recording the range, elevation, azimuth, and exact state of the platform, reflected power information from objects is also recorded for storage in the map (this reflected power is specific to the state of the platform). This takes advantage of the fact that different materials in the environment surrounding the platform reflect RF signals with different magnitudes in the direction of the radar sensor. For example, metal reflects most of the power from the radar, while wood reflects almost nothing. Furthermore, the orientation of metal objects relative to the radar RF signal also contributes to how much power is reflected in the direction of the radar. Therefore, reflected power can be used as the third or fourth dimension of a 2D or 3D occupancy grid, respectively. Using reflected power can reduce uncertainty about what the state of the platform should be (thereby reducing the likelihood of generating many good matches between the local and global maps).

[0392] Contemplated Embodiments The presented solution for crowdsourced map building enables Software as a Service (SaaS) technologies. Map building technologies can provide different functions using map data. Centralizing the map creation process in a cloud / web service can allow data received from any vehicle to be processed and used to generate updated maps. These cloud / web services for map building enable a SaaS model that goes beyond simple real-time positioning and navigation solutions. An important example of such a service is the ability to continuously evaluate map data for environmental changes and update map areas that have undergone changes. Some of the services that can be enabled include fully or partially accessing an area from the map, accessing feature quality for an area in the map, indicating the potential location accuracy (confidence) for a map area, comparing mapped areas over time to evaluate changes, improving map quality and functionality by adding more data over time, building map updates over time, and other implementations.

[0393] This disclosure describes the body frame such that x is forward, y is positive toward the right side of the body, and the z axis is positive downward. It is contemplated that any body frame definition can be used for application of the methods and apparatus described herein.

[0394] It is contemplated that the techniques of the present disclosure may be used with automatic zero velocity or static period detection with its possible updates and inertial sensor bias recalculation, a nonholonomic update module, advanced modeling and / or calibration of inertial sensor errors, deriving their possible measurement updates from GNSS when appropriate, automatic assessment of GNSS solution quality and detection of degraded performance, automatic switching between loosely and tightly coupled integration schemes, a navigation solution that may optionally utilize an assessment of each visible GNSS satellite when in tightly coupled mode, and finally, possibly with a backward smoothing module that uses any type of backward smoothing technique and either runs post-mission or in the background on buffered data within the same mission.

[0395] It is further contemplated that the techniques of the present disclosure can also be used in conjunction with transport mode techniques or motion mode detection techniques to establish a transport mode, thereby enabling detection of a pedestrian mode among other modes, such as a driving mode, etc. When a pedestrian mode is detected, the methods presented in the present disclosure can be operable to determine a mismatch between the device and a pedestrian.

[0396] It is further contemplated that the techniques of the present disclosure can be used with a navigation solution further programmed to run a routine in the background that simulates an artificial outage in the absolute navigation information and estimates parameters of another instance of the state estimation technique used in the solution in the navigation module to optimize the accuracy and consistency of the solution. The accuracy and consistency are evaluated by comparing the temporary background solution during the simulated outage with a reference solution. The reference solution may be one of the following examples: absolute navigation information (e.g., GNSS), a forward integrated navigation solution on the device that integrates available sensors with absolute navigation information (e.g., GNSS) and possibly optional speed or velocity readings, or a backward smoothed integrated navigation solution that integrates available sensors with absolute navigation information (e.g., GNSS) and possibly optional speed or velocity readings. The background processing can be executed either on the same processor as the forward solution processing or on a separate processor that can communicate with the first processor and read stored data from a shared location. The results of the background processing solution can benefit the real-time navigation solution in its future runs (i.e., real-time runs after the background routines have finished running), for example, by having improved values ​​for the parameters of the forward state estimation technique used for navigation in this module.

[0397] It is further contemplated that the techniques of the present disclosure can also be used with navigation solutions that are further integrated with maps (such as street maps, indoor maps or models, or any other environmental maps or models for applications where such maps or models are available) in addition to the different core uses of map information and map or model matching routines described above. Map or model matching can further improve navigation solutions during degradation or interruption of absolute navigation information (such as GNSS). In the case of model matching, a sensor or group of sensors that acquires information about the environment can be used, such as a laser range finder, a camera and vision system, or a sonar system. These new systems can either be used as additional help to improve the accuracy of navigation solutions during issues (degradation or absence) of absolute navigation information, or can completely replace absolute navigation information in some applications.

[0398] It is further contemplated that the techniques of the present disclosure, when operating in either a tightly coupled manner or a hybrid loose / tightly coupled option, can also be used with navigation solutions that do not need to be tied to utilizing pseudorange measurements (calculated from codes rather than carrier phase, and thus referred to as code-based pseudoranges) and Doppler measurements (used to derive pseudorange rates). GNSS receiver carrier phase measurements can be used as well, for example (i) as an alternative method of calculating range in place of code-based pseudoranges, or (ii) to enhance range calculations by incorporating information from both code-based pseudoranges and carrier phase measurements, such enhancement being carrier-smoothed pseudoranges.

[0399] It is further contemplated that the techniques of this disclosure may also be used in conjunction with navigation solutions that rely on very tight integration between GNSS receivers and other sensor readings.

[0400] It is further contemplated that the techniques of the present disclosure can also be used with navigation solutions that use various wireless communication systems that can also be used for positioning and navigation, either as additional assistance (more useful when GNSS is unavailable) or as a substitute for GNSS information (e.g., for applications where GNSS is not applicable). Examples of these wireless communication systems used for positioning include those provided by cell phone towers and signals, radio signals, digital television signals, WiFi, or WiMAX. For example, in cell phone-based applications, absolute coordinates from cell phone towers and the range between an indoor user and the tower may be used for positioning, whereby range can be estimated by various methods that calculate the time of arrival or time difference of arrival of the nearest cell phone positioning coordinates. A method known as Enhanced Observed Time Difference (E-OTD) can be used to obtain known coordinates and range. The standard deviation of range measurements may depend on the type of oscillator used in the cell phone and cell tower timing equipment, as well as transmission losses. WiFi positioning can be performed in a variety of ways, including, but not limited to, time of arrival, time difference of arrival, angle of arrival, received signal strength, and fingerprinting techniques, among others, all of which provide different levels of accuracy. Wireless communication systems used for positioning may use different techniques for modeling errors in range, angle, or signal strength from wireless signals and may use different multipath mitigation techniques. All of the above considerations are equally applicable to other wireless positioning technologies based on wireless communication systems, among others.

[0401] It is further contemplated that the techniques of the present disclosure can also be used with navigation solutions that utilize aiding information from other mobile devices. This aiding information can be used as additional aiding (more useful when GNSS is unavailable) or as a substitute for GNSS information (e.g., for applications where GNSS-based positioning is not applicable). One example of aiding information from other devices can be relying on wireless communication systems between different devices. The underlying idea is that a device with a better positioning or navigation solution (e.g., with GNSS having good availability and accuracy) can help a device with degraded or unavailable GNSS obtain an improved positioning or navigation solution. This aiding relies on the known location and wireless communication system of the assisting device(s) to position the device(s) with degraded or unavailable GNSS. This contemplated variation refers to one or both of the following situations(s): (i) a device or devices with degraded or unavailable GNSS utilize the methods described herein to obtain assistance from other devices and communication systems, and (ii) an assisting device with available GNSS, and therefore a good navigation solution, utilizes the methods described herein. Wireless communication systems used for positioning may rely on different communication protocols and may rely on different methods such as time of arrival, time difference of arrival, angle of arrival, and received signal strength, among others. Wireless communication systems used for positioning may use different techniques for modeling errors in ranging and / or angle from wireless signals and may use different multipath mitigation techniques.

[0402] The above-described embodiments and techniques may be implemented in software as various interconnected functional blocks or separate software modules. However, this is not required, and these functional blocks or modules may equivalently be aggregated into a single logical device, program, or operation with unclear boundaries. In any case, the functional blocks and software modules or interface features implementing the above-described embodiments may be implemented alone or in combination with other operations in either hardware or software, either entirely within the device, or in conjunction with the device and other processor-enabled devices that communicate with the device, such as a server.

[0403] While several embodiments have been shown and described, it will be understood by those skilled in the art that various changes and modifications can be made to these embodiments without altering or departing from their scope, spirit, or function. The terms and expressions employed in the foregoing specification are used herein as terms of description and not of limitation, and there is no intention in the use of such terms and expressions to exclude equivalents of the features shown and described or portions thereof, recognizing that the present disclosure is defined and limited only by the scope of the following claims. [Explanation of symbols]

[0404] 100 devices 102 processors 104 memory 106 Sensor Assembly 108 External Sensor 110 Bus 112 radar readings 114 Integration Module 116 Absolute Navigation Information 118 Communication Module 200 devices 202 Host Processor 204 memory 206 Motion Processing Unit (MPU) 208 Sensor Processor 210 memory 212 Internal Sensor 214 External Sensor 216 Auxiliary Sensor 218 Bus 220 Bus 222 radar readings 224 Integration Module 226 Absolute Navigation Information 228 Communication Module

Claims

1. 1. A method of constructing a map of an area around at least one route traversed by a mobile platform using an integrated navigation solution for a device in said mobile platform, comprising: a) acquiring motion sensor data from a sensor assembly of the device; b) obtaining absolute navigation information for said platform; c) acquiring radar measurements from at least one radar on said platform; d) generating an integrated navigation solution based at least in part on the motion sensor data and the absolute navigation information, the integrated navigation solution providing at least a position and an orientation output; e) projecting the radar measurements onto the area from the position and orientation output of the integrated navigation solution; f) constructing a map for the area using the projected radar measurements; and A method comprising:

2. The method of claim 1 , wherein constructing the map comprises aggregating projected radar measurements for multiple position and orientation outputs of an integrated navigation solution along the at least one route.

3. i) multiple position and orientation outputs of the integrated navigation solution along multiple routes; ii) multiple position and orientation outputs of the integrated navigation solution from multiple mobile platforms; iii) multiple position and orientation outputs of the integrated navigation solution from multiple moving platforms along multiple routes; aggregating the projected radar measurements for at least one of The method of claim 2.

4. 4. The method of claim 2 or 3, further comprising determining a confidence level for a subsection of the area of ​​the map constructed based at least in part on the plurality of position and orientation outputs, the determined confidence level representing the potential accuracy of a position output of another integrated navigation solution subsequently derived using the constructed map.

5. 5. The method of claim 4, further comprising determining an uncertainty of the integrated navigation solution of claim 1, wherein the determined confidence level of the subsection of the area of ​​the map is based at least in part on the determined uncertainty.

6. The method of claim 4 , wherein the determined confidence level of the subsection of the area of ​​the map is based at least in part on the absolute navigation information.

7. The determined confidence level of the subdivision of the area of ​​the map is based at least in part on an accuracy of the absolute navigation information, the accuracy of the absolute navigation information being determined as follows: a) the accuracy of the absolute navigation information is used aggregated from all routes that a mobile platform takes through the subsection of the map; and b) the accuracy of the absolute navigation information is used around the subsection of the map from each distinct route passing through the subsection of the map; used in at least one of The method of claim 4.

8. The method of claim 4 , wherein the determined confidence level of the subsection of the area of ​​the map is based at least in part on a geometry that depends on a configuration of the at least one radar of the platform.

9. The method of claim 4 , wherein the determined confidence level of the subsection of the area of ​​the map is based at least in part on a number of features detected using the projected radar measurements.

10. The method of claim 4 , wherein the determined confidence level of the subsection of the area of ​​the map is based at least in part on a route traversed by a mobile platform.

11. The determined reliability is i) all subdivisions of said area; ii) a subdivision of the area with detection results determined from the projected radar measurements; and iii) subdivisions of said area along said at least one route traversed by a moving platform; based at least in part on one of The method of claim 4.

12. The method of claim 1 , wherein the at least one route is configured to provide desired coverage of the area.

13. The method of claim 1 , further comprising: rejecting the absolute navigation information when degradation is detected when generating the integrated navigation solution.

14. The integrated navigation solution comprises: i) forward processing; ii) backward processing, and iii) Combining forward and backward processing; based on at least one of The method of claim 1.

15. The method of claim 1 , wherein the integrated navigation solution is based on a smoothing process.

16. 2. The method of claim 1 , wherein constructing the map includes at least one of using the projected radar measurements to assign new probability decisions to subsections of the area of ​​the map and to update existing probability decisions for subsections of the area of ​​the map.

17. The method of claim 1 , further comprising determining an occupancy probability for a subsection of the area of ​​the map.

18. The method of claim 1 , further comprising cleaning the map based at least in part on trajectories passing through subdivisions of the area of ​​the map.

19. The output of the integrated navigation solution is improved based at least in part on the received motion sensor data using a nonlinear state estimation technique, wherein a prediction phase involving a system model is used to propagate predictions regarding a state of the platform, and an update phase involving at least one measurement model relating measurements to the state is used to update the state of the platform, the nonlinear state estimation technique includes using a nonlinear measurement model for radar measurements, in which the integration of the motion sensor data and the radar measurements is tightly coupled, and the generating i) using the acquired motion sensor data in the non-linear state estimation technique; ii) directly integrating the radar measurements by updating the nonlinear state estimation technique using the nonlinear measurement model and the constructed map; Including, the output of the improved integrated navigation solution is used to project the radar measurements onto the area to construct an improved map. The method of claim 1.

20. The measurement model: i) a radar range-based model based at least in part on a probability distribution of measured ranges using the estimated state of the platform and the constructed map; ii) a radar nearest object likelihood model based at least in part on a probability distribution of distances to objects detected using the radar measurements, the estimated state of the platform, and nearest object identifications from the constructed map; and iii) a radar map matching model based at least in part on a probability distribution derived by correlating a global map derived from the constructed map with the projected radar measurements and the map constructed using the integrated navigation solution; and iv) a radar closed-form model based at least in part on the relationship between the integrated navigation solution and the constructed ranges to objects from the map; and at least one of:

20. The method of claim 19.

21. 1. A system for constructing a map of an area around at least one route traversed by a mobile platform using an integrated navigation solution for devices in said mobile platform, comprising: a) a device having a sensor assembly configured to output motion sensor data; b) a source of absolute navigation information; c) at least one radar configured to output radar measurements of said platform; d) at least one processor coupled to acquire the motion sensor data and the radar measurements; wherein the at least one processor: i) generating an integrated navigation solution based at least in part on the motion sensor data and the absolute navigation information, the integrated navigation solution providing at least a position and orientation output; ii) projecting the radar measurements onto the area from the position and orientation output of the integrated navigation solution; iii) constructing a map for the area using the projected radar measurements; It works like this, system.

22. 22. The system of claim 21, wherein the at least one processor is operative to construct the map by aggregating projected radar measurements for multiple position and orientation outputs of an integrated navigation solution along the at least one route.

23. the processor: i) multiple position and orientation outputs of the integrated navigation solution along multiple routes; ii) multiple position and orientation outputs of the integrated navigation solution from multiple mobile platforms; iii) multiple position and orientation outputs of the integrated navigation solution from multiple moving platforms along multiple routes; and further operating to aggregate projected radar measurements for at least one of:

23. The system of claim 22.

24. 24. The system of claim 22 or 23, wherein the at least one processor is further operative to determine a confidence level for a subsection of the area of ​​the map constructed based at least in part on the plurality of position and orientation outputs, the determined confidence level representing the potential accuracy for a position output of another integrated navigation solution subsequently derived using the constructed map.

25. the at least one processor is further operative to use a nonlinear state estimation technique to improve a position output of the integrated navigation solution based at least in part on the motion sensor data, wherein a prediction phase involving a system model is used to propagate predictions regarding a state of the platform, and an update phase involving at least one measurement model relating measurements to the state is used to update the state of the platform, the nonlinear state estimation technique includes using a nonlinear measurement model for radar measurements, wherein the integration of the motion sensor data and the radar measurements in the nonlinear state estimation technique is tightly coupled, and the generating i) using the acquired motion sensor data in the non-linear state estimation technique; ii) directly integrating the radar measurements by updating the nonlinear state estimation technique using the nonlinear measurement model and the constructed map; Including, the at least one processor uses the output of the improved integrated navigation solution to project the radar measurements onto the area to construct an improved map.

22. The system of claim 21.

26. 22. The system of claim 21, wherein the sensor assembly includes an accelerometer and a gyroscope.

27. 22. The system of claim 21, wherein the sensor assembly is implemented as a micro-electro-mechanical system (MEMS).

Images