Kalman filters for parking

Bộ lọc Kalman cho đỗ xe

A Kalman filter sounds like graduate-school machinery, but the idea behind it is almost embarrassingly practical. You maintain a guess about where you are and how wrong that guess might be. When you take a step, you predict a new position and deliberately widen your uncertainty because steps are never perfect. When a sensor offers a measurement — compass heading, perhaps, or a hint that you stopped moving — you blend that measurement with your guess, weighted by how much you trust each source. The math is linear algebra; the philosophy is humility.

Wheria uses a reduced extended Kalman filter on a four-element state vector: horizontal position x and y in local meters, heading theta in radians, and a slowly drifting gyroscope bias term. We call it extended because heading updates involve trigonometry, but the implementation on phone is still a handful of matrix multiplies per step, not a supercomputer job. The filter never runs in isolation; it sits between step detection and the confidence badge you see in Find mode.

Prediction when the user walks

Each detected step triggers a prediction. Position advances by stride length times the cosine and sine of current heading. The covariance matrix P — which encodes how uncertain we are about each state element — grows by a process noise matrix Q. Q is not pulled from a textbook appendix; we tuned it from field logs in real garages. Stride uncertainty sits around five centimeters standard deviation per step. Heading process noise is about two degrees per step when compass trust is high, and closer to eight degrees in steel-heavy zones where magnetometer corrections are unreliable.

That asymmetry matters. In an open-air slab garage, compass measurements arrive frequently and the filter tightens heading after almost every few steps. In a spiral ramp wrapped in rebar, we inflate measurement noise for magnetometer updates by an order of magnitude, which effectively tells the filter to believe the gyroscope integration between steps and treat compass spikes as suspicious guests rather than honored advisors.

Measurements and when to believe them

Magnetometer heading enters as a measurement z equal to the arctangent of the horizontal field components. The update adjusts theta and pulls P down when the field magnitude and inclination look like Earth's expected range. When they do not — when steel columns rotate effective north by forty degrees without the user turning — the filter increases measurement noise R for magnetometer observations and the heading correction becomes gentle instead of violent.

Barometer floor hints are handled more cautiously. A pressure drop since parking can suggest you descended one slab, but elevators, HVAC gusts, and weather fronts also move pressure. We often surface barometer inference as parallel UI guidance — "likely B3" with a plus-or-minus one floor band — rather than hard-fusing altitude into the Kalman state where a false lock would corrupt horizontal confidence users rely on for distance-to-car.

Zero-velocity updates are a classic pedestrian trick: if no step has registered for about two seconds, assume the user stood still and damp any legacy velocity drift in integration paths that still carry speed states from earlier prototypes. It is a small stabilizer, not a cure for escalators.

Why not a particle filter or full SLAM

Particle filters represent uncertainty with clouds of hypothetical positions and weights. They shine when ambiguity is multimodal — you genuinely do not know whether the user turned left or right at a fork. They also burn CPU and complicate parity between Swift and Kotlin implementations that must behave identically for ktuyen's regression matrix. For walks under roughly one hundred twenty meters in a single garage, an extended Kalman filter with adaptive measurement noise has been sufficient. We would revisit particles if we added map matching to painted parking lines in open lots where fork ambiguity is common.

Simultaneous localization and mapping with camera or lidar is even farther from our constraints. Parking garages change signage, lighting, and parked car layouts daily. Phone-only SLAM without infrastructure beacons fails the offline, no-account, privacy-first requirements described in our on-device post.

What users actually see

The trace of the position covariance block maps to the green, yellow, and red confidence chip on the Find screen. When uncertainty grows beyond a threshold, the UI switches from a precise arrow to a coarse distance ring: walk about sixty meters, then reassess. Outdoors, if a GPS segment is still valid, the two-by-two position block of P can draw an uncertainty ellipse on the map instead of a lying pinpoint dot.

If you want to play with the matrices interactively, Phyzix includes a Kalman demonstration that steps through predict and update with sliders for Q and R. The full indoor pipeline — step detection, compass fusion, barometer hints — is documented in indoor navigation when GPS dies. Step odometry details live in step detection and IMU odometry; magnetometer failure modes in magnetometers and compass nightmares.

Bộ lọc Kalman nghe như máy móc của trường sau đại học, nhưng ý tưởng đằng sau gần như thực dụng đến mức xấu hổ. Bạn giữ một phỏng đoán về vị trí và mức sai có thể của phỏng đoán đó. Khi đi thêm một bước, bạn dự đoán vị trí mới và cố ý nới rộng độ không chắc vì bước chân không bao giờ hoàn hảo. Khi cảm biến đưa một phép đo — có thể heading la bàn, hoặc dấu hiệu bạn đứng yên — bạn trộn phép đo với phỏng đoán, có trọng số theo mức tin từng nguồn. Toán là đại số tuyến tính; triết lý là khiêm tốn.

Wheria dùng bộ lọc Kalman mở rộng rút gọn trên vector trạng thái bốn phần tử: vị trí ngang x và y theo mét local, heading theta theo radian, và một thành phần bias con quay trôi chậm. Gọi là mở rộng vì cập nhật heading có lượng giác, nhưng triển khai trên điện thoại vẫn chỉ vài phép nhân ma trận mỗi bước, không phải việc siêu máy tính. Bộ lọc không chạy cô lập; nó nằm giữa phát hiện bước và chip confidence bạn thấy ở chế độ Tìm.

Dự đoán khi người dùng đi bộ

Mỗi bước được phát hiện kích hoạt một bước dự đoán. Vị trí tiến theo độ dài sải nhân cos và sin của heading hiện tại. Ma trận hiệp phương sai P — mã hóa mức không chắc về từng thành phần trạng thái — tăng nhờ ma trận nhiễu quá trình Q. Q không lấy từ phụ lục sách giáo khoa; team tune từ log thực địa trong hầm thật. Độ không chắc sải chân khoảng năm centimet độ lệch chuẩn mỗi bước. Nhiễu quá trình heading khoảng hai độ mỗi bước khi la bàn đáng tin, và gần tám độ ở vùng nhiều thép nơi hiệu chỉnh từ kế không đáng tin.

Bất đối xứng đó quan trọng. Hầm sàn mở, đo la bàn đến thường xuyên và bộ lọc siết heading sau vài bước. Ramp xoắn bọc cốt thép, team phồng nhiễu đo từ kế lên một bậc độ lớn — bộ lọc tin tích phân con quay giữa các bước và coi spike la bàn như khách đáng ngờ thay vì cố vấn được vinh danh.

Phép đo và khi nào tin

Heading từ kế vào như phép đo z bằng arctangent của thành phần từ trường ngang. Bước cập nhật chỉnh theta và kéo P xuống khi độ lớn field và độ nghiêng giống dải Trái Đất kỳ vọng. Khi không — cột thép xoay "bắc hiệu dụng" bốn mươi độ mà user không quẹo — bộ lọc tăng nhiễu đo R cho quan sát từ kế và hiệu chỉnh heading trở nên nhẹ thay vì giật mạnh.

Gợi ý tầng từ áp kế được xử lý thận trọng hơn. Áp suất giảm từ lúc đỗ có thể gợi ý xuống một sàn, nhưng thang máy, gió HVAC và front thời tiết cũng làm áp thay đổi. Team thường đưa suy luận áp kế ra giao diện song song — "có thể B3" với dải cộng trừ một tầng — thay vì fusion cứng độ cao vào state Kalman, nơi lock sai làm hỏng confidence ngang mà user dựa vào để biết còn bao xa tới xe.

Cập nhật vận tốc-zero là mẹo pedestrian kinh điển: nếu không có bước trong khoảng hai giây, giả định user đứng yên và triệt drift vận tốc legacy trên đường tích phân còn mang state tốc độ từ prototype cũ. Đó là chốt nhỏ, không phải thuốc cho thang cuốn.

Vì sao không particle filter hay SLAM đầy đủ

Particle filter biểu diễn không chắc bằng đám mây vị trí giả thuyết có trọng số. Chúng mạnh khi mơ hồ đa mode — thật sự không biết user rẽ trái hay phải ở ngã ba. Chúng cũng ngốn CPU và làm khó parity Swift/Kotlin phải hành xử giống hệt cho ma trận regression của ktuyen. Với quãng đi dưới khoảng một trăm hai mươi mét trong một hầm, Kalman mở rộng với nhiễu đo thích ứng đã đủ. Team sẽ xem lại particle nếu thêm map matching vạch sơn bãi hở nơi mơ hồ ngã ba phổ biến.

Định vị và dựng bản đồ đồng thời bằng camera hay lidar còn xa hơn ràng buộc của app. Hầm đổ biển báo, ánh sáng, bố trí xe mỗi ngày. SLAM chỉ điện thoại không beacon hạ tầng không qua yêu cầu offline, không tài khoản, privacy-first trong bài on-device.

User thực sự thấy gì

Trace của khối covariance vị trí ánh xạ sang chip confidence xanh, vàng, đỏ trên màn Tìm. Khi không chắc vượt ngưỡng, giao diện chuyển từ mũi tên chính xác sang vòng khoảng cách thô: đi khoảng sáu mươi mét, rồi đánh giá lại. Ngoài trời, nếu đoạn GPS còn hợp lệ, khối hai-by-hai vị trí của P có thể vẽ ellipse không chắc trên bản đồ thay vì chấm giả chính xác.

Muốn chơi ma trận tương tác, Phyzix có demo Kalman bước qua predict và update với slider Q và R. Pipeline indoor đầy đủ — phát hiện bước, fusion la bàn, gợi ý áp kế — nằm trong dẫn đường indoor khi GPS chết. Chi tiết odometry bước ở phát hiện bước và IMU; chế độ hỏng từ kế ở từ kế và cơn ác mộng la bàn.