Tuning LiDAR, Radar, and Adding Camera-Based Distance to the Stack

The hardware is all there. The LiDAR is spinning, the radar is streaming tracks, and everything lands in one dashboard on the Jetson. But getting the sensors to a point where they actually mean something where a warning is a warning and silence is silence that took a lot longer than getting them plugged in.

This post covers what I changed on the LiDAR filtering, the radar signal processing pipeline, how YOLO estimates distance from bounding boxes, and the sensor health monitoring layer I built on top of all of it. No hardware changes, no car yet. Just bench tuning which turns out to be where most of the real work is.

LiDAR: Removing the Noise Layer by Layer

The LD06 fundamentals packet parsing, coordinate system, three-zone layout are in Building the Sensor Foundation. This picks up where that left off. Once the sensor was physically working, the problem became obvious almost immediately it was far too easy to make it lie.

Throwing Away What Doesn't Matter

The LD06 scans 360 degrees but for forward collision detection that's 320 degrees of wasted processing and noise. I cut the active field of view down to ±20° forward only. Narrow-FOV filtering is standard practice in automotive LiDAR perception it reduces irrelevant return density and keeps the decision logic focused on what the vehicle is actually heading toward.^[1]

On top of that I added a 20cm minimum distance floor. The LD06 has a physical blind zone close to its lens and readings inside it are unreliable. I also added a secondary geometric distance check because occasionally a point slips through the angular filter but still originates very close to the sensor origin. That catches the stragglers.

Minimum Points Per Zone

The forward arc is split into left, centre, and right lanes. Each zone reports its minimum distance. But a single point is not enough to count as a real obstacle.

LiDAR point clouds naturally contain stray returns reflections off shiny surfaces, dust, cable edges, electrical noise. Research into LiDAR noise filtering confirms that single-point spurious returns are common enough that minimum-count thresholds are a standard first line of defence before any obstacle decision is made.^[2]

I scaled the minimum threshold with distance. Closer objects produce denser return clusters so I demand more points. Distant objects naturally produce fewer, so fewer are required. This keeps sensitivity high at range without being sloppy up close.

The Persistence Filter

Even with FOV cut and minimum-point check, the raw zone state still flickered. An obstacle would appear, clear, appear again all within a few scans. That's useless for any kind of braking decision.

The fix was a persistence filter: an obstacle needs to appear in 3 consecutive scans to trigger an alert, and needs to be absent for 4 consecutive scans before the alert clears. The asymmetry is deliberate dropping an alert one frame early is fine, but dropping it mid-obstacle because of a single noisy return is not. This hysteresis approach asymmetric raise and clear thresholds shows up widely in LiDAR processing pipelines precisely because transient false positives are such a consistent problem at the sensor level.^[3]

Knowing When the LiDAR Itself Is Struggling

One thing I spent time on that wasn't in the original design was detecting when the LiDAR is compromised as opposed to just absent. There are two failure modes that look superficially the same: physically disconnected, and present but unable to see anything useful rain, dirt, physical blockage.

Disconnected is easy to catch: the serial port throws an error and the timestamp goes stale. But if the lens is blocked while data is still flowing, the sensor keeps sending packets they just have no valid forward-zone points in them.

I detect this by counting consecutive frames where raw data is arriving but zero points survive the forward filter. After 15 frames of that, I flag the sensor as degraded rather than disconnected. That distinction is what controls whether the radar steps in as a backup for static obstacles it only does that when the LiDAR is physically impaired, not when it's simply unplugged.

Radar: Building From Raw Signal to Trustworthy Tracks

The radar setup is covered in Building the Sensor Foundation  BGT60TR13C on the DreamHAT+, Pi 5, signal processing pipeline, UDP streaming to the Jetson. What that post documents is the initial working pipeline. What this covers is everything I changed to make it reliable enough to actually act on.

Clearing the Background With MTI

The biggest practical challenge with radar indoors is that it sees everything: walls, the floor, the car chassis, nearby furniture. All of it produces persistent strong reflections. Before anything useful can be detected, that background has to go.

The standard technique is MTI Moving Target Indicator. Each new frame has the previous frame subtracted from it. Static reflections have near-zero Doppler shift and disappear. Moving objects produce a phase shift across frames and survive. MTI is fundamentally a high-pass filter on the Doppler dimension and it's the standard method for clutter suppression precisely because static clutter and moving targets occupy different parts of that spectrum.^[4]

I set the MTI memory coefficient at 0.92, which means the background model updates very slowly. That makes it aggressive at suppressing static returns. The tradeoff is roughly 15 frames of settling time after startup, but once settled the floor, walls, and chassis are largely gone from the signal.

CFAR Detection

After MTI, I run CFAR Constant False Alarm Rate detection. Rather than a fixed threshold, CFAR slides a window across the processed signal and flags anything that exceeds the local noise floor by a set margin. Mine is set at 16 dB.

CFAR is the standard dynamic thresholding approach in radar because it keeps the false alarm rate roughly constant regardless of how the noise floor varies across the scene.^[5] The 16 dB threshold is deliberately strict. Weaker returns noise leaking through MTI, multipath clutter don't make it through. Real targets at relevant distances do.

Tracking: Turning Detections Into Objects

Raw CFAR detections jump around frame to frame even on a genuine target. A nearest-neighbour tracker turns them into stable entities: each new detection gets associated with the closest existing track by range and velocity. Unmatched detections start tentative tracks. A track needs 3 consecutive confirmed frames before it's reported upstream, and gets deleted after 3 frames without a match.

That 3-frame minimum age does a lot of work. A single-frame ghost detection a transient reflection or noise peak never reaches the fusion system at all. The track dies before it becomes visible.

Approach Confirmation Gate

Even tracked objects produce noisy velocity measurements. A single frame where a track briefly shows negative velocity doesn't mean something is genuinely closing. So I added an approach confirmation gate: a track needs consistently negative velocity for 3 consecutive frames before it's allowed to trigger a collision warning.

Real approaching objects produce stable, consistent velocity signatures. Noise bounces around and fails the consistency requirement. This single change had the most visible impact on false alarm rate during bench testing tracks that appeared for a few frames without genuinely closing were filtered out before they could raise an alarm.

Time-to-Collision Thresholds

Once a track is confirmed approaching, alarm level comes from TTC: range divided by closing velocity. Under 1.5 seconds to impact, or already within 0.5m, is IMMINENT. Under 3 seconds or within 1.2m is CAUTION. There's a range fallback for when velocity is near zero anything very close gets flagged regardless.

What Radar Deliberately Ignores

In normal operation, radar only alarms on objects moving toward the car. Stationary obstacles are the LiDAR's job entirely. This is a direct consequence of the MTI filter it exists specifically because static returns would otherwise overwhelm the signal. Using radar for stationary detection after MTI is fighting against the pipeline's own design.

The one exception is when the LiDAR is physically impaired. In that situation radar will re-evaluate whether any static tracks are dangerously close. But only while the car is actually moving a stopped car sees everything as stationary and would produce constant spurious alerts.

YOLO: Estimating Distance From a Bounding Box

The camera's job in this system is covered in YOLOv8 on Jetson CUDA getting GStreamer working, the torchvision mock, running inference on CUDA. But the camera does more than just confirm whether something is there. It also produces a rough distance estimate from the bounding box itself, and that estimate feeds directly into the collision decision.

Why Bounding Box Size Tells You Something About Distance

The basic principle is projective geometry. As an object gets further away, it occupies fewer pixels in the image. More specifically, the height of the bounding box in pixels is inversely proportional to the real-world distance. Double the distance and the box height roughly halves. This relationship between apparent size and distance is well established in monocular camera-based object detection work using bounding box dimensions as a proxy for depth is a documented approach for lightweight distance estimation when you don't have a depth sensor.^[6]

The IMX477 has a known focal length and sensor geometry. Combined with assumed real-world object heights for common objects a person is roughly 1.7m tall, a low obstacle maybe 0.3m you can map the pixel height of a YOLO bounding box to an estimated physical distance. It's not as precise as LiDAR, but it doesn't need to be. Its job is to produce a distance band: IMMINENT, CAUTION, or SAFE.

How It Works in Practice

When YOLO detects an object in the forward zone the centre 40% of the frame the bounding box height in pixels gets fed into a simple distance formula. The formula uses the camera's focal length and the assumed real-world height of the detected class to estimate range.

That estimated range then maps to a collision band using the same IMMINENT / CAUTION / SAFE thresholds as the LiDAR and radar. Objects detected as very large in frame (box height above a set fraction of total frame height) sit in IMMINENT. Mid-sized detections sit in CAUTION. Smaller detections further back sit in SAFE.

The camera also checks the mean brightness of each frame. If the scene is too dark mean brightness falling below a threshold of 40 the camera flags itself as degraded and clears its current detections immediately. There's no point passing a distance estimate based on a barely-visible detection into the fusion layer. When that happens, the system knows not to rely on camera output and adjusts accordingly.

What the Camera's Distance Estimate Is Actually Used For

This is worth being clear about. The camera distance estimate is a cross-validation input, not a primary ranging source. It doesn't replace the LiDAR for geometry. What it does is provide an independent second opinion. If the LiDAR says CAUTION but the camera's bounding-box estimate also puts something in the CAUTION or IMMINENT band, that's a confirmed detection. If the LiDAR fires but the camera sees nothing, the alert gets suppressed as noise exactly as described in YOLOv8 on Jetson CUDA.

The distance band from the camera becomes more significant when the LiDAR is degraded. With the geometry sensor impaired, the bounding box estimate rough as it is gives the fusion layer something to work with rather than flying blind.

Sensor Health Monitoring: Knowing When to Trust the System

Running three sensors simultaneously creates a problem that doesn't exist with one: how do you know when the system itself is compromised? A single-sensor system is either working or it isn't. With three sensors, you can have partial failure one sensor offline, two sensors degraded, any combination of the above and you need the system to communicate that clearly rather than silently report a confidence it doesn't have.

This led to building an explicit health monitoring layer. It runs alongside the fusion logic and maintains a live status for each sensor independently.

Disconnected vs Degraded: Two Different Things

The most important distinction in the health layer is between a sensor being disconnected and a sensor being degraded. They feel similar from the outside you're not getting useful data in either case but they have completely different causes and different appropriate responses.

Disconnected means the sensor has stopped sending data. The timestamp on the last received packet goes stale past a threshold and the sensor is marked offline. This usually means a cable pulled out, a serial port error, or the streamer process crashing on the Pi.

Degraded means data is arriving but the sensor can't see anything useful. The LiDAR is spinning and sending packets but the lens is blocked and nothing is passing the forward filter. The camera is running but the scene is too dark to detect anything. The sensor isn't broken it's impaired by its environment.

The system handles these differently. A disconnected LiDAR means the radar doesn't step in for static objects a disconnected sensor could mean anything and you shouldn't change behaviour based on it. A degraded LiDAR is a specific physical situation weather or blockage where switching to radar static fallback makes sense.

Per-Sensor Status Labels

The GUI health bar at the top of the dashboard shows each sensor's state individually:

LiDAR can be: online, disconnected (stale timestamp), or degraded (data arriving, zero forward points for 15+ frames).

Radar can be: online, or disconnected (no UDP packets from Pi within the stale threshold). Radar doesn't have a degraded state when MTI is running normally and CFAR finds nothing, that just means nothing is there. The distinction between no signal and no detections isn't meaningful for radar the way it is for LiDAR.

Camera can be: online, disconnected (frame acquisition failing), or degraded (mean brightness below threshold 40). When degraded, current detections are immediately cleared I don't want a stale bounding box from a frame where the camera could barely see anything feeding into the fusion decision.

The System-Level Warning Bar

Individual sensor status is useful for debugging. But what matters operationally is what the combined state means. I built a system-level summary bar that maps the combination of individual sensor states into one of four messages:

ALL SENSORS ONLINE everything healthy, full capability.

SENSOR OFFLINE one sensor is disconnected or degraded, with a note on what capability is reduced. Amber.

X + Y DISCONNECTED  two sensors are out, with a note on what mode the system is running in. Red.

ALL SENSORS OFFLINE SYSTEM BLIND full red. Nothing is working.

The colour and message update every frame. If the LiDAR comes back online mid-session the bar immediately goes green again. If the camera goes dark, it drops to amber within one second.

What Happens When Multiple Sensors Fail at Once

The most important behaviour to get right wasn't the single-sensor case that's straightforward. It was the case where two sensors fail simultaneously.

The original fusion logic had a bug here: if both LiDAR and camera were simultaneously impaired, the function could fall through to returning SAFE. That's exactly backwards. If I can't see anything, the right answer is not SAFE it's CAUTION at minimum.

The fix was to floor the output at CAUTION whenever both LiDAR and camera are impaired or disconnected at the same time. The radar can still upgrade that to IMMINENT if it detects something approaching. But the system will never silently report SAFE when it's effectively blind on two of its three sensors.

How It All Fits Together

The final collision verdict is always the worst case across all active sensors. Radar says IMMINENT, LiDAR says SAFE the output is IMMINENT. Neither sensor can vote the result downward.

The camera's bounding-box distance estimate contributes to the fusion output but only as a confirming signal it doesn't trigger alerts in isolation. It either confirms what LiDAR or radar is already saying, or it suppresses a LiDAR detection that has no visual backing.

Radar works completely independently of LiDAR and camera for moving threats. A person walking toward the car gets detected whether or not the LiDAR is running. The sensors are designed to complement each other, not to depend on each other.

The health monitoring layer sits above all of this and communicates to the system — and to whoever is watching the dashboard exactly what state the perception stack is in at any given moment.

What I Learned From This

The most useful insight from this phase is that false alarms are just as dangerous as missed detections. A system that cries wolf constantly gets ignored or bypassed. A lot of what I built isn't about sensitivity it's about trustworthiness.

The second thing is about failure modes. Every sensor has situations where it stops working correctly. The interesting question isn't how to prevent that you can't it's how to detect it and hand off gracefully. The distinction between disconnected and degraded, between a wiring fault and a weather event, mattered far more than I expected when I started.

And the bounding-box distance approach from YOLO is a good reminder that you don't always need the most precise tool. A rough distance band from a bounding box height, combined with LiDAR and radar, gives you enough cross-validation to make the system significantly more robust than either sensor alone.

Next up is getting everything physically on the car and seeing how the pipeline holds up when it's actually moving. Click on the link below to watch the video that shows the system running live on the bench.

https://drive.google.com/file/d/1xem-SiJ1d_Ls9DsGIGkBK_CP46yDv7VK/view?usp=sharing

Github : https://github.com/rh960/Sensor_Driven_Digital_Twin_For_Collison_Prevention_in_Autonomous_Systems

References

[1]     Roriz, R. et al. (2021). "Positioning and perception in LIDAR point clouds." Signal Processing: Image Communication. sciencedirect.com/science/article/pii/S1051200421002323

[2]    Shi, S. et al. (2020). "3D Point Cloud Processing and Learning for Autonomous Driving." arXiv:2003.00601. arxiv.org/abs/2003.00601

[3]    Li, Y. et al. (2024). "Algorithm for Point Cloud Dust Filtering of LiDAR for Autonomous Vehicles in Mining Area." MDPI Sustainability. mdpi.com/2071-1050/16/7/2827

[4]    Richards, M. A. (2005). Fundamentals of Radar Signal Processing. McGraw-Hill. (MTI reference via MathWorks: mathworks.com/help/radar/ug/ground-clutter-mitigation)

[5]     Huang, X. et al. (2024). "Exploring Radar Data Representations in Autonomous Driving: A Comprehensive Review." arXiv:2312.04861. arxiv.org/abs/2312.04861

Vajgl, M. & Hurtik, P. (2022). "Dist-YOLO: Fast Object Detection with Distance Estimation." Applied Sciences 12(3), 1354. mdpi.com/2076-3417/12/3/1354