คณิตศาสตร์ของ Error-State Extended Kalman Filter (ES-EKF) และ Quaternion
การนำ Quaternion และ Error-State Extended Kalman Filter (ES-EKF) มาใช้ร่วมกัน เป็นแนวทางมาตรฐานอุตสาหกรรมในระบบ INS/GNSS ของอากาศยานและยานยนต์อัตโนมัติ เพื่อหลีกเลี่ยงปัญหา Gimbal Lock ของมุมออยเลอร์ และเลี่ยงปัญหา Singularity ของ Covariance Matrix เมื่อใช้ 4 ตัวแปรของ Quaternion ในฟิลเตอร์โดยตรง
การแทนทิศทางด้วย Quaternion
นิยามควอเทอร์เนียนหนึ่งหน่วย (Unit Quaternion)
กำหนด q = [q_w, q_x, q_y, q_z]^T โดยที่ขนาดเวกเตอร์ ||q|| = 1
การหมุนเวกเตอร์
การหมุนเวกเตอร์ v จากพิกัดตัวรถ (Body Frame: b) ไปยังพิกัดอ้างอิงโลก (Navigation Frame: n) คำนวณผ่านเมทริกซ์การหมุน R(q) หรือผลคูณควอเทอร์เนียน v^n = q ⊗ [0, v^b]^T ⊗ q*
อนุพันธ์ตามเวลาของ Quaternion
สัมพันธ์กับความเร็วเชิงมุมจากไจโรสโคป ω ด้วยสมการ dq/dt = (1/2) * q ⊗ [0, ω]^T
โครงสร้างและการทำงานของ ES-EKF
ES-EKF แยกสถานะจริงออกเป็น 2 ส่วน คือ x = x_nominal ⊕ x_error
- Nominal State (16 มิติ)
ประกอบด้วย ตำแหน่ง (3), ความเร็ว (3), ควอเทอร์เนียน (4), ไบแอสความเร่ง (3), และไบแอสไจโรสโคป (3) คำนวณแบบไม่เป็นเชิงเส้นด้วยการอินทิเกรตข้อมูล IMU ความถี่สูง 100–200 Hz - Error State (15 มิติแบบ Minimal Representation)
ประกอบด้วย ผลต่างตำแหน่ง (3), ผลต่างความเร็ว (3), เวกเตอร์มุมคลาดเคลื่อนขนาดเล็ก δθ (3 มิติ แทนที่จะเป็นควอเทอร์เนียน 4 มิติ เพื่อป้องกัน Singularity), ผลต่างไบแอสความเร่ง (3), และผลต่างไบแอสไจโรสโคป (3) - ขั้นตอน Predict (Time Update)
Nominal State อินทิเกรตการเคลื่อนที่เชิงกายภาพล่วงหน้า ขณะที่ Error Covariance Matrix P (ขนาด 15×15) จะถ่ายทอดความไม่แน่นอนผ่าน Transition Matrix F_k ที่เป็นเชิงเส้น - ขั้นตอน Update และ Injection
เมื่อได้รับพิกัดจาก GNSS ระบบจะคำนวณ Error State ผ่าน Kalman Gain นำค่าชดเชยที่ได้ไปบวกแก้ให้กับ Nominal State (โดยส่วนมุมจะคูณ Quaternion ชดเชยและทำ Normalize) จากนั้นทำการรีเซ็ตค่าคาดหวังของ Error State ให้กลับเป็นศูนย์ทันที (Error Reset)