Patent Description:
One or more implementations relate generally to using optical sensors to resolve vehicle heading issues.

An automatic steering system may steer a vehicle along a desired path. The steering system may use gyroscopes (gyros), accelerometers and a global navigation satellite system (a) to determine the location and heading of the vehicle. While steering along the desired path, the vehicle may need to stop. While the vehicle is stopped, the heading estimate will drift due to drift in the gyros.

When the vehicle starts moving again, the automatic steering system will have poor steering performance until the heading and roll estimations are corrected. If the heading is offset, the controller will try to correct this and if the roll is offset then the projection from the antenna position to the ground will be offset. These offsets will slowly be corrected for example by an extended Kalman filter. However, until the offsets are corrected the vehicle will not steer as precisely and have a wiggling behavior. In very low speed operations the estimation of heading is also challenged due to noisy and/or delayed heading information from a global navigation satellite system (GNSS).

A typical agricultural GNSS steering control system uses GNSS positioning and an inertial measurement unit (IMU) for heading information or uses a dual antenna to calculate heading based on the position of the two antennas. Due to crabbing, such as on a slope, the vehicle heading is not always aligned with the direction of the course over ground. GNSS also requires a good line of sight to satellites. Trees, buildings, windmills etc. can cause the GPS position to degrade or not be available. This is important for farmers that need precise vehicle control systems. Products on the market try to solve this problem by wheel odometry, inertial navigation systems (INS) and getting the best out of the available GNSS even though it has degraded, such as from real-time kinematic (RTK) fix to RTK float, etc..

Dual antenna systems may measure the heading and roll as long as there is high precision GNSS available independent of velocity. The extra antenna hardware however makes the system more expensive than single antenna systems. The precision of the heading is also limited by the length of the baseline between the two or more antennas and the precision of the GNSS signal. This can be a problem for certain vehicles, such as narrow vineyard tractors.

Single antenna systems rely on gyros and accelerometers to measure the roll and yaw of the vehicle. The yaw is used together with the GNSS course over ground to get a good a heading of the vehicle for control of the vehicle heading. Since the course over ground is not the same as the heading of the vehicle due to crabbing, a single GNSS system will not be able to directly measure the crabbing like a dual antenna GNSS system.

The roll and heading are also used for projecting the GNSS antenna position readings to the point on the vehicle to be controlled. Typically, the vehicle operator is concerned about the accuracy on the ground. The gyros and accelerometers drift over time and are especially affected by temperature, shocks and vibration, and depending on the technology and quality, also have a bias instability that is difficult to calibrate. These biases are compensated by the GNSS course over ground information based on the Doppler effect and/or low-pass filtered delta values between the last n position measurements from GNSS. Both course over ground sources from GNSS are poor at low speed and not available at a standstill.

As mentioned above, gyroscopes are used for navigation, guidance, and stabilization and/or pointing of many manned and unmanned systems designed for commercial, industrial, and military applications. From game controllers to smartphones, and from remote stabilized weapons to driverless vehicles, gyros and inertial measurement units (IMUs) perform a number of vital navigation, guidance, and positioning functions within these systems.

With the tremendous variety of applications comes an equally wide array of performance grades in gyros and IMUs. Consumer grade gyros such as those used in video game controllers, smartphones, tablets, and automobile airbag systems exist on the low-end of both performance and cost. More demanding applications such as weapons systems, driverless vehicles, and navigation in GPS/GNSS-denied environments require a much higher grade of performance. The performance capabilities and accuracy requirements determine which technology is integrated into a specific system.

Micro-electro-mechanical systems (MEMS) gyros offer smaller size and weight and less power consumption than other gyroscopes. MEMS are capable of withstanding high non-operating shock levels, and in general offer a lower cost than other gyro technologies. Some weaknesses of MEMS gyros and inertial systems lie in critical performance parameters such as higher angle random walk/noise, which is an extremely important performance criterion in stabilization and positioning systems. In addition, MEMS gyros have higher bias instability, which results in a degraded navigation or stabilization/pointing solution. Thermal sensitivity of MEMS gyros and inertial systems also impact their bias and scale factor performance. These attributes are important to both stabilization and navigation applications.

For instance, <CIT> shows a system for guiding a vehicle. The system comprises a location determining receiver for collecting location data for the vehicle and a vision module collecting vision data for the vehicle. A location quality estimator estimates the location quality data of the location data and the vision module estimates vision quality data of the vision data. By selecting a mixing ratio for the vision data and location data based on the determined quality data, the vehicle is guided by the vision data and/or location data.

From <CIT> a method and a system is known for collecting positional information for a movable object. The method includes the steps of receiving data from a first and a second sensing system coupled to the movable object, determining a weight value for the data from the second sensing system based on a strength of the signal received by the sensor of the second sensing system, and calculating the positional information of the movable object.

<CIT> shows a global positioning system and dead reckoning (DR) integrated navigation system. The system includes a GPS receiver coupled to a moving object for generating GPS navigation information of said moving object, a DR system coupled to said moving object for calculating DR navigation information of said moving object, and a filter coupled to said GPS receiver and said DR system for calculating navigation information of said moving object.

Moreover, <CIT> discloses a method and a system for integrating an inertial guidance system (IGS) and a GPS receiver. A predictive filter measures a signal quality from the GPS receiver and provides parameter estimates by appropriately weighting signal data from the GPS receiver and the IGS system.

<CIT> shows a method for the initialization of an inertial navigation system using information obtained from an image. Positional and orientation information about the object and with respect to the object are obtained from the image.

From <CIT> a method and an apparatus for determining inaccurate GPS samples in a set of GPS samples are known. The method includes the steps of obtaining GPS samples, obtaining a first estimation of the trajectory based on the GPS samples, obtaining a second estimation of the trajectory based on measurements made by an inertial measurement unit, and comparing the first and second estimation.

The included drawings are for illustrative purposes and serve to provide examples of possible structures and operations for the disclosed inventive systems, apparatus, methods and computer-readable storage media. These drawings in no way limit any changes in form and detail that may be made by one skilled in the art without departing from the scope of the invention as defined in the appended claims.

<FIG> is a side view and <FIG> is a top view of a vehicle <NUM>. Vehicle <NUM> includes a steering control system <NUM> that solves the problem of poor heading after a standstill start/stop, low speed and/or poor GNSS. Steering control system <NUM> includes a camera system that uses one or more cameras <NUM> to solve the problem of losing GNSS vehicle heading information during a standstill. Cameras <NUM> identify features <NUM> in a field of view <NUM> and continuously tracks features <NUM> to maintain a heading of vehicle <NUM> without drifting.

The explanation below may refer to GNSS and global positioning systems (GPS) interchangeably and both refer to any locating system, such as a satellite or cellular positioning system, that provides a latitude and longitude and/or a position relative to true north.

In one example, camera <NUM> is mounted in the front top center of the cabin of vehicle <NUM>. Camera <NUM> is looking forward and has a relatively wide field of view to view features <NUM> close to vehicle <NUM> and on the horizon. In other examples, cameras <NUM> are located inside of the vehicle cabin and/or on a front end of vehicle <NUM>. Of course, cameras <NUM> may be located in any other location of vehicle <NUM>.

Cameras <NUM> do not necessarily have to look forward. Cameras <NUM> also may look to the side or backwards of vehicle <NUM>. Cameras <NUM> could also operate as a surround view or <NUM> degree view and could also include an omnidirectional camera that takes a <NUM> degree view image.

Control system <NUM> may operate algorithms that calculate the pose and trajectory of cameras <NUM> by chronologically analyzing images in scenes or frames. These algorithms process the captured images in chronological order and track movements of the images from one frame to a next frame. Based on the tracked movement of the images, or sparse features in the images, the change in both position and orientation of camera <NUM> can be determined from image to image. The image movements tracked by camera <NUM> are used by a control algorithm in control system <NUM> in combination with GNSS positions from GNSS <NUM> and IMU turn rates and accelerations from IMU <NUM> for determining a heading and position of vehicle <NUM> that are then used for steering vehicle <NUM>.

One example algorithm used for calculating the pose and trajectory of a camera is described in <CIT>.

Other algorithms may only output orientation and not pose.

A sensor may include a camera <NUM> in combination with a three-dimensional (3D) sensor so tracked features <NUM> are also localized in 3D by a direct measurement. Control system <NUM> can also detect vehicle orientation and pose based on a single camera <NUM> providing monocular visual odometry that uses special initialization based on assumptions about the scene to solve scale problems.

Monocular camera measurements of features <NUM> are relative. An absolute scale value can be obtained by control system <NUM> making certain assumptions about the scene, such as a planar scene, to recover the scale or alternatively use known points in 3D for recovering scale. The 3D sensors may include integrated stereo cameras, radar, LIDAR, or any other 3D sensor. Control system <NUM> also may calculate the orientation and pose of vehicle <NUM> based on a 3D sensor alone without an additional camera <NUM>. However, for agricultural fields a combination of camera <NUM> and a 3D sensor may provide more accurate vehicle orientation and pose measurements.

Control system <NUM> may also use visual odometry to create a map of the area, often referred to as simultaneous localization and mapping (SLAM). Optical sensors, such as camera <NUM> can localize when placed in the map at a later time. This map may be geographically located with GNSS when available from GNSS <NUM>. Visual features <NUM> may be stored in a map based on GNSS. In one example, the map is stored online for easy updating by the different vehicles working in the field.

Visual odometry may drift as a function of primarily distance travelled. If a location in the map is revisited, control system <NUM> may use a loop closure operation to optimize the map and reduce drift. Control system <NUM> also may reduce drift by using GNSS readings when available to give absolute inputs for the map creation process.

The created map may include a pointcloud map with 3D points presenting the 3D structure of the environment. This is a popular method for laser based systems. For camera based systems, control system <NUM> may augment the map position information with image information, such as feature descriptions, that allow a more robust tracking from image to image and also provide better re-localization in the map.

Thus, control system <NUM> may generate the vehicle pose from either a visual odometry solution and/or a SLAM solution. As mentioned above, visual odometry and SLAM are known to those skilled in the art and are therefore calculating vehicle orientation and pose based on odometry or SLAM are not described in further detail.

<FIG> shows reference directions for a moving vehicle. Vehicle <NUM> may have a heading <NUM>. However, do to physical circumstances, such as the slope of a hill, vehicle <NUM> may travel along a different path referred to as course over ground <NUM>. The angle between vehicle heading <NUM> and course over ground heading is referred to as the crab angle <NUM>. Course over ground <NUM> is also referred to as the velocity heading <NUM>.

Vision data from camera <NUM> may provide a relative position, relative orientation, relative course over ground, and speed in a vehicle coordinate frame. Known vision algorithms also may deliver confidence values associated with the vision based position, orientation, course over ground, and speed estimations. The vision data from camera <NUM> may drift primarily based on a distance travelled by vehicle <NUM>.

The GNSS <NUM> provides GNSS data that may provide an absolute position, speed and course over ground <NUM> in a north-east coordinate frame. The speed and course over ground provided by GNSS <NUM> is typically noisy at low vehicle speeds. IMU <NUM> provides pitch, roll and yaw rates and accelerations. The integration of turn rates measured by IMU <NUM> typically drift over time.

<FIG> is a block diagram explaining how vision data <NUM> from optical camera <NUM> is fused with GNSS data <NUM> from GNSS <NUM> and IMU data <NUM> from IMU <NUM>. Control system <NUM> also may operational data <NUM> associated with vehicle <NUM>. For example, operational data <NUM> may include the vehicle speed from a speedometer or calculated from GNSS <NUM> or camera system <NUM>. Operational data <NUM> also may include a distance of vehicle travel and a time of vehicle travel.

Control system <NUM> operates a fusion orientation algorithm <NUM> that uses vision data <NUM>, GNSS data <NUM>, and IMU data <NUM> to more accurately calculate the heading and/or location of vehicle <NUM> after a standstill start/stop, low speed and/or poor GNSS. The description below may refer to heading and orientation interchangeably. Any reference to calculating vehicle heading below also may include calculating the vehicle position.

<FIG> shows in more detail the fusion operations performed by the steering control system. Referring to <FIG> and <FIG>, in operation 150A, control system <NUM> may receive vision data <NUM> from camera <NUM>, GNSS data <NUM> from GNSS <NUM>, and IMU data <NUM> from IMU <NUM>. In operation 150B, control system <NUM> receives speed data <NUM> from a vehicle speedometer or from any of sensors <NUM>, <NUM>, and/or <NUM>.

Control system <NUM> assigns weights to vision data <NUM>, GNSS data <NUM>, and IMU data <NUM> in operations 150C, 150D, and 150E, respectively, based on vehicle operating parameters. For example, control system <NUM> in operation 150C may assign vision data <NUM> a <NUM>% weighting when vehicle <NUM> is at a standstill and assign vision data <NUM> a higher weight at low vehicle speeds.

Control system <NUM> in operation 150C may further weight vision data <NUM> based on a confidence factor coming from a vision tracking algorithm applied to vision data <NUM>. In other words, control system <NUM> may weight vision data <NUM> based on a reliability factor determined by vision tracking algorithms. As explained above, vision tracking algorithms that calculate vision tracking reliability are known to those skilled in the art and is therefore not explained in further detail.

In operation 150D, control system <NUM> may assign a higher weight to GNSS heading, speed, and course over ground data <NUM> when vehicle <NUM> travels at higher speeds. Control system <NUM> in operation <NUM> may assign lower weightings to GNSS data <NUM> at slower vehicle speeds and assign a zero weighting when the vehicle is stopped.

The different sensor data especially from vision and GNSS might already come with a noise characteristic in form of e.g. a covariance matrix. Ideally each sensor would already provide a noise characteristic that included the speed dependency. In this ideal case the Kalman filter could just fuse directly without any further weighting scheme. This kind of information is however often not provided to the user of a GNSS system.

In operation 150E, control system <NUM> may assign a higher weight to IMU data <NUM> when vehicle <NUM> is traveling as higher speeds. This may account for the less noise and drift that GNSS and IMU data may have at higher vehicle speeds and shorter time periods, respectively.

In operation 150F, control system <NUM> fuses together the weighted data from sensors <NUM>, <NUM>, and <NUM> to provide an improved estimate of the vehicle orientation (heading). Fusion orientation <NUM> in <FIG> weights and blends together data from camera sensor <NUM>, GNSS sensor <NUM>, and IMU sensor <NUM> to mitigate the problems with each individual sensor source. For example, fusion operation <NUM> mitigates the increased noise and uncertainty in single antenna GPS velocity heading measurements at reduced vehicle speeds and/or mitigates the increased drift and uncertainty in IMU yaw angle estimates associated with integrating inertial rate measurements with time.

<FIG> shows one example control system used for steering a vehicle based on the fused sensor data described above. An operator may enter a desired path <NUM> into control system <NUM>. Control system <NUM> may include controllers K<NUM>, K<NUM>, and K<NUM> for controlling different characteristics of vehicle <NUM>, sensors <NUM>-<NUM>, and valve steering controller <NUM>, respectively. For example, controller K<NUM> may control a heading control loop, controller K<NUM> may control a heading control loop, and controller K<NUM> may control a curvature control loop. Feedback control systems that include gain control coefficients K<NUM>, K<NUM>, and K<NUM> are known to those skilled in the art and is therefore not described in further detail.

Desired path <NUM> is fed through model controllers K<NUM>, K<NUM>, and K<NUM> and then into a hydraulic valve steering system <NUM> for that steers vehicle <NUM>. Desired vehicle path <NUM> is compared with a projected position <NUM> of vehicle <NUM>. The difference is fed back into controller K<NUM>.

Vision data <NUM>, GNSS data <NUM>, and IMU data <NUM> from sensors <NUM>, <NUM>, and <NUM> is fused together as described above and further below by fusion operation <NUM> and compared with the output from controller K<NUM>. The difference of the comparison is feed into controller K<NUM>. The output from valve steering system <NUM> is fed back and compared with the output of controller K<NUM>. The output of controller K<NUM> are then fed back into valve steering <NUM> for steering vehicle <NUM>.

The fused heading data output from fusion operation <NUM> allows control system <NUM> to project more accurate positions <NUM> of vehicle <NUM> and more accurately steer vehicle <NUM>. The scheme used by fusion operation <NUM> can be accomplished with a number of different algorithms or combinations of algorithms, such as, but not limited to, Kalman filtering and complementary filtering.

<FIG> shows one example scheme for fusing GNSS data <NUM> and visual heading data <NUM> using a complementary filter with the filter cross-over frequencies scheduled with speed. A speed to frequency mapping operation <NUM> may convert the speed of vehicle <NUM> into a frequency signal. For example, mapping <NUM> may apply a weighting to a vehicle speed identified in the GPS data to generate a frequency value. The GPS heading data <NUM> is into a low pass filter <NUM> and visual heading data <NUM> is feed into a high pass filter <NUM>.

As mentioned above, GPS velocity heading information <NUM> becomes less reliable at slower speeds and requires more low-pass filtering. When vehicle <NUM> is stationary or traveling at very low speeds, GPS data <NUM> does not provide accurate heading information and is completely filtered by low pass filter <NUM>. At lower speeds, less GPS heading data <NUM> is provided from low pass filter <NUM> and more visual heading data <NUM> is provided from high pass filter <NUM>. At higher speeds, less visual heading data <NUM> is provided from high pass filter <NUM> and more GPS heading data <NUM> is provided from low pass filter <NUM>.

Visual heading data <NUM> does not give an absolute heading (relative to north) but can provide strong heading change information, even when the vehicle is stationary. The GPS velocity heading data <NUM> can provide absolute (i.e. relative to north) information while the visual heading data <NUM> can provide strong heading change information.

As the speed of vehicle <NUM> drops towards zero, the frequency from mapping operation <NUM> decreases in complementary filters <NUM> and <NUM> until fused heading <NUM> fully relies on visual heading data <NUM>. As the speed of vehicle <NUM> increases, the cross-over frequency from frequency mapping operation <NUM> increases causing fused heading <NUM> to rely more on the now stronger GPS heading information <NUM>.

<FIG> shows how fusion operation <NUM> may generate a fused heading <NUM>. Fused heading <NUM> derived in <FIG> is fed into a low pass filter <NUM>. A gyroscope yaw rate from IMU data <NUM> is fed into an integrator <NUM> to produce a yaw angle. The yaw angle is fed into a high pass filter <NUM>.

Inertial sensor data <NUM> is a useful source of high rate heading rate measurements. However, heading estimates from integrating the inertial rate measurements in integration <NUM> suffer from drift due to accumulation of small errors in the heading rate measurements. This drift needs to be constrained with absolute heading measurements. When only GPS heading measurements are used as a source of heading corrections, no heading information can be obtained when the vehicle is stationary, in this condition the heading estimates fully rely on the inertial information <NUM> so the heading estimates will drift.

Complementary filters <NUM> and <NUM> handle this data fusion. Fused heading <NUM> from the complementary filters fusing GPS and visual headings in <FIG> provide the absolute heading reference. The absolute heading reference is then used to constrain the drift from integrating the gyro heading rate information <NUM>. The absolute heading reference <NUM> can be used even when vehicle <NUM> is stationary, due to the complementary information obtained from the different visual and GNSS heading information. For example, complementary filters <NUM> and <NUM> may filter out the low frequency IMU data <NUM> with less inaccurate information due to low frequency drift and may filter less of the high frequency IMU data <NUM> which is more accurate.

<FIG> shows for explanatory purposes how a Kalman filter <NUM> can be used for fusing GPS heading data <NUM> and visual heading data <NUM> with IMU measurement data <NUM>. Kalman filter <NUM> may include a prediction stage that uses the IMU measurements <NUM> to predict a next forward vehicle heading <NUM>. Kalman filter <NUM> may include an update stage <NUM> that uses GPS heading <NUM> and visual heading <NUM> to constrain the drift in IMU data <NUM>.

Kalman filter <NUM> may fuse heading measurements <NUM>, <NUM>, and <NUM> at the same time with each measurement weighted according to an estimated uncertainty around the measurement. This allows Kalman filter <NUM> to internally correct a current internal heading estimate using the best available sensor source to produce an overall improved estimate.

As explained above, the estimated uncertainty of GPS heading measurements <NUM> may be based on the speed of vehicle <NUM>. The estimated uncertainty of visual heading data <NUM> may be constant relative to speed but may vary over time or distance.

<FIG> shows for explanatory purposes another technique for determining heading where GPS and visual measurements are not fused together. GPS heading measurement data <NUM> and visual heading measurement data are both fed into a switching operation <NUM> performed by fusion orientation operation <NUM>. Switching operation <NUM> also receives a current vehicle speed.

Instead of combining heading measurements <NUM> and <NUM>, switching operation <NUM> selects one of measurements <NUM> or <NUM> with a current lowest uncertainty (highest certainty). For example, at low speeds or stationary conditions, switching operation <NUM> may select visual heading measurements <NUM> for determining the vehicle heading. At higher speeds, switching operation <NUM> may select GPS heading measurements <NUM> for determining the vehicle heading.

<FIG> shows a cascaded fusion operation. GPS heading measurement data <NUM> and visual heading measurement data <NUM> is fed into the complimentary low pass and high pass filters <NUM> and <NUM>, respectively, as previously shown in <FIG>. The fused heading data <NUM> from complimentary filters <NUM> and <NUM> and IMU measurement data <NUM> are fed into complimentary low pass and high pass filters <NUM> and <NUM>, respectively, shown in <FIG>. The cascaded complimentary filtering fuses all of the different sensor heading data <NUM>-<NUM> together into a fused heading <NUM>.

<FIG> shows an example complementary filter cascaded with a Kalman filter. As shown above in <FIG>, GPS heading measurement data <NUM> and visual heading measurement data is fused together by complementary filters <NUM> and <NUM>. The output of complementary filters <NUM>, <NUM> is fed into Kalman filter <NUM> and IMU measurement data <NUM> is fed into Kalman filter <NUM>. Kalman filter <NUM> then estimates the vehicle heading and/or other vehicle states based on fused GPS measurement data <NUM>, visual measurement data <NUM>, and IMU measurement data <NUM>.

In a normal speed range above <NUM> to <NUM>/h the GNSS derived course over ground can be used to initialize the system similar to initialization of the IMU. Since the GNSS course over ground is noisy at low speed below approximately <NUM>/h, control system <NUM> may not be able to initialize the heading directly from GNSS data. Control system <NUM> may use GNSS heading data stored for a previous driven path to align the vehicle heading with the course over ground and the true north heading.

<FIG> shows how control system <NUM> may align a vision path to a GNSS path after a path history length. GNSS <NUM> may detect a vehicle POS over a path <NUM> indicated with X's and camera <NUM> may detect a vehicle pos and orientation over a path <NUM> indicated by circles with arrows.

Control system <NUM> may initialize and align the relative vision heading indicated by path <NUM> to the north GNSS heading indicated by path <NUM>. For example, a polynomial or spline function <NUM> is fitted to a previous traveled distance, such as the last <NUM> meters driven by vehicle <NUM>. Polynomial or spline function <NUM> provides robust information about the course over ground traveled by vehicle <NUM> even at low speed. Based on the GNSS course over ground <NUM> and a possible crab angle determined by visual data from camera <NUM>, control system <NUM> estimates and corrects a bias on vision heading <NUM>. The vision bias may be frequently updated to avoid big jumps in the vision measurements.

In one example, visual heading measurements <NUM> are produced relative to some initial heading condition while GPS velocity heading measurements <NUM> are relative to absolute north. In order to use visual measurements <NUM> when GPS velocity heading measurements <NUM> are unavailable, control system <NUM> aligns visual heading <NUM> and true north. The true north alignment is also updated to account for any accrued visual drift while vehicle <NUM> is in operation.

Control system <NUM> may use the visual data to determine the crab angle between GNSS path <NUM> and an actual course over ground path of vehicle <NUM>. Control system <NUM> may also assume no crab angle for appropriate vehicle types and when appropriate operating conditions are met. Control system <NUM> also may measure crab angle using a sensor that measures the velocity of the vehicle in the body frame compared with the velocity of the vehicle in the GPS frame.

Control system <NUM> derives GPS velocity heading measurements <NUM> from the motion of the GNSS antenna <NUM> attached to vehicle <NUM>. Control system <NUM> derives visual heading measurements <NUM> based on the orientation vehicle <NUM> is facing. These two angles (vehicle heading vs velocity heading) do not need to be the same. For example, vehicle <NUM> might not be traveling in the direction it is facing. A clear example of this is when vehicle <NUM> is traveling in reverse. While traveling in reverse, vehicle heading and velocity heading measurements are <NUM> degrees away from each other. A less serve case would be when vehicle <NUM> is side-slipping due to vehicle under or over-steer in a turn or has a crab angle due to the slope of the terrain.

Control system <NUM> uses the crab angle to align visual heading measurements <NUM> with the GPS measurements <NUM>. It is possible to measure this sideslip angle by estimating the velocity vector of the direction of motion relative to the vehicle frame (i.e. motion direction of the camera). In another example, control system may use an assumption about the motion such as a zero side-slip condition. Once determined, control system <NUM> may use the sideslip angle to align visual heading measurements <NUM> with GPS velocity heading measurements <NUM>.

<FIG> shows how control system <NUM> estimates the GPS course heading based on a time history of GPS positions. Control system <NUM> may not be able to use GPS velocity heading <NUM> at low speeds due to the amount of noise on the measurements. Visual heading data <NUM> must first be aligned with GNSS data <NUM>. In other words, both GNSS heading data <NUM> and visual heading data <NUM> are operating in a same north-east frame of reference.

Control system <NUM> may use a Kalman filter that stores a history of previous headings and positions of vehicle <NUM>. The Kalman filter than combines the heading and position history data with the crab angle derived from the visual data <NUM> to determine a visual heading alignment offset <NUM> between GNSS data <NUM> and visual data <NUM>.

Once the visual to GPS alignment is determined, control system <NUM> continuously corrects the visual heading measurements <NUM> based on the alignment offset <NUM>. Control system <NUM> then uses the corrected/aligned visual heading measurement <NUM> in-place of GPS measurements <NUM> when the GPS measurements <NUM> are unavailable.

<FIG> shows how a Kalman filter is used to estimate vehicle heading from GPS and visual measurements. A GPS velocity heading 142A and a GPS position heading 142B from the GNSS sensor <NUM> are input into Kalman filter <NUM>. A visual heading <NUM> from camera <NUM> and a body crab angle <NUM> determined from visual data <NUM> are also input into Kalman filter <NUM>. Kalman filter <NUM> generates the heading <NUM> as described above.

Internally Kalman filter <NUM> can estimate the visual to GPS alignment <NUM> when both GNSS <NUM> and camera <NUM> measurements sources are available. The top block in <FIG> shows a GPS and visual mode where both GPS measurement data <NUM> and visual measurement data <NUM> is available and used by Kalman filter <NUM> to determine heading <NUM>. Kalman filter <NUM> at the same time also uses crab angle <NUM> to determine alignment <NUM> that aligns visual heading <NUM> with GPS heading <NUM>.

The bottom block in <FIG> shows a visual only mode where GPS heading data <NUM> is no longer available when the vehicle is stopped or operating at a very slow speed, such as less than <NUM>/h. If GPS measurements <NUM> become unreliable, Kalman filter <NUM> can use the last estimated alignment <NUM> with visual heading measurements <NUM> to keep heading estimate <NUM> constrained.

The camera system described above allows the vehicle to continue auto steering with high precision even if GNSS is lost for a shorter period of time. This is especially relevant on headlands where there is often a tree line that can block the view to the satellites. With automatic turning on the headland it is desired to have good positioning also on the headland. The control system may use visual data <NUM> in combination with SLAM for a field and compare items identified in the map with visually detected features to eliminate the drift problem.

Items causing blockage or causing multipath of GNSS data is often high obstructions like buildings, trees, windmills, power line towers, etc. These obstructions are also very good visual landmarks that are different from other visual features in the field. A field is often driven in the same path year after year. This is also a benefit for the system since the visual features will most often need to be recognized from similar directions.

Some of the operations described above may be implemented in software and other operations may be implemented in hardware. One or more of the operations, processes, or methods described herein may be performed by an apparatus, device, or system similar to those as described herein and with reference to the illustrated figures.

"Computer-readable storage medium" (or alternatively, "machine-readable storage medium") used in control system <NUM> may include any type of memory, as well as new technologies that may arise in the future, as long as they may be capable of storing digital information in the nature of a computer program or other data, at least temporarily, in such a manner that the stored information may be "read" by an appropriate processing device. The term "computer-readable" may not be limited to the historical usage of "computer" to imply a complete mainframe, mini-computer, desktop, wireless device, or even a laptop computer. Rather, "computer-readable" may comprise storage medium that may be readable by a processor, processing device, or any computing system. Such media may be any available media that may be locally and/or remotely accessible by a computer or processor, and may include volatile and non-volatile media, and removable and non-removable media.

Examples of systems, apparatus, computer-readable storage media, and methods are provided solely to add context and aid in the understanding of the disclosed implementations. It will thus be apparent to one skilled in the art that the disclosed implementations may be practiced without some or all of the specific details provided. In other instances, certain process or methods also referred to herein as "blocks," have not been described in detail in order to avoid unnecessarily obscuring the disclosed implementations.

Claim 1:
A control system for fusing different sensor data together to determine an orientation of a vehicle, comprising:
a hardware processor to:
receive visual heading data (<NUM>) for the vehicle (<NUM>) from a camera system (<NUM>);
receive global navigation satellite system (GNSS) heading data (<NUM>) from a GNSS system (<NUM>);
receive inertial measurement unit (IMU) heading data (<NUM>) for the vehicle (<NUM>) from an IMU (<NUM>);
monitor a speed of the vehicle (<NUM>);
assign weights to the visual heading data (<NUM>), the GNSS heading data (<NUM>) and the IMU heading data (<NUM>) based on operating conditions of the vehicle (<NUM>) that vary accuracy associated with the visual heading data (<NUM>), the GNSS heading data (<NUM>) and the IMU heading data (<NUM>), wherein
- lower weights are assigned to the GNSS heading data (<NUM>) when the vehicle (<NUM>) is traveling at a speed below a given value,
- a higher weight is assigned to the GNSS heading data (<NUM>) when the vehicle (<NUM>) is traveling at higher speeds,
- a zero weight is assigned to the GNSS heading data (<NUM>) and only the visual heading data (<NUM>) is used to determine the heading (<NUM>) of the vehicle (<NUM>) when the vehicle (<NUM>) is stopped; and
use the weighted visual heading data (<NUM>), the weighted GNSS heading data (<NUM>) and the weighted IMU heading data (<NUM>) to determine a heading (<NUM>) of the vehicle (<NUM>), wherein the vehicle (<NUM>) is steered based on the heading (<NUM>) of the vehicle (<NUM>).