การแสดงผลและการดีบัก EKF

ขณะนี้ฉันกำลังดีบักและปรับ EKF (ตัวกรองขยาย Kalman) ภารกิจคือหุ่นยนต์เคลื่อนที่แบบคลาสสิคที่ใช้ติดตามสถานที่สำคัญคือเครื่องหมาย AR

บางครั้งฉันก็ประหลาดใจว่าการวัดบางอย่างมีผลต่อการประมาณ เมื่อฉันดูและคำนวณตัวเลขและเมทริกซ์ที่เกี่ยวข้องฉันสามารถหาวิธีดำเนินการขั้นตอนการอัปเดตสิ่งที่และสาเหตุที่เกิดขึ้นจริง แต่สิ่งนี้น่าเบื่อมาก

ดังนั้นฉันจึงสงสัยว่ามีใครใช้เทคนิคการหลอกลวงหรือการสร้างภาพอย่างชาญฉลาดเพื่อให้รู้สึกดีขึ้นว่าเกิดอะไรขึ้นในขั้นตอนการอัปเดต EKF หรือไม่

อัปเดต # 1 (จะเฉพาะเจาะจงมากขึ้นและแสดงการประมาณแรกของสิ่งที่ฉันมีในใจ)

สิ่งที่ฉันกำลังมองหาเป็นวิธีที่จะเห็นภาพขั้นตอนการอัปเดตเดียวในแบบที่ทำให้ฉันรู้สึกว่าองค์ประกอบแต่ละส่วนของการวัดมีผลต่อแต่ละองค์ประกอบของรัฐอย่างไร

แนวคิดแรกของฉันคือวางแผนการวัดและการทำนายพร้อมกับเวกเตอร์บางตัวที่นำมาจากเมทริกซ์เค เวกเตอร์จาก K แสดงให้เห็นว่าเวกเตอร์นวัตกรรม (การวัด - การทำนายการวัดไม่ใช่พล็อต) จะส่งผลกระทบต่อส่วนประกอบของรัฐอย่างไร

ขณะนี้ฉันกำลังทำงานกับ EKF ที่สถานะเป็นแบบ 2D (x, y, มุม) และการวัดก็เป็นแบบสองมิติ

ในภาพที่แนบมา (เปิดในหน้า / แท็บใหม่เพื่อดูความละเอียดเต็ม) เวกเตอร์ (ปรับขนาด) K (1,1: 2) (ไวยากรณ์ MATLAB เพื่อรับซับเมทริกซ์จากเมทริกซ์ 3x3) ควรให้ความคิดว่าภาพแรกเป็นอย่างไร ส่วนประกอบของสถานะ EKF จะเปลี่ยนไปด้วยเวกเตอร์นวัตกรรมปัจจุบัน K (2,1: 2) วิธีที่องค์ประกอบที่สองของ EKF จะเปลี่ยน ฯลฯ ในตัวอย่างนี้เวกเตอร์นวัตกรรมมีองค์ประกอบ x ที่ค่อนข้างใหญ่และสอดคล้องกับ เวกเตอร์ K (2,1: 2) - องค์ประกอบที่สองของรัฐ (พิกัด y) จะเปลี่ยนไปมากที่สุด

ปัญหาหนึ่งในพล็อตนี้คือมันไม่ได้ให้ความรู้สึกว่าองค์ประกอบที่สาม (มุม) ของเวกเตอร์นวัตกรรมมีผลต่อสถานะอย่างไร องค์ประกอบแรกของรัฐเพิ่มขึ้นเล็กน้อยซึ่งตรงกันข้ามกับสิ่งที่ K (1: 1: 2) บ่งบอก - องค์ประกอบที่สามของนวัตกรรมทำให้สิ่งนี้เกิดขึ้น แต่ปัจจุบันฉันไม่สามารถมองเห็นสิ่งนี้ได้

การปรับปรุงครั้งแรกคือการมองเห็นว่าองค์ประกอบที่สามของนวัตกรรมมีผลต่อรัฐอย่างไร ถ้าอย่างนั้นก็เป็นการดีที่จะเพิ่มข้อมูลความแปรปรวนร่วมเพื่อรับความรู้สึกว่า K matrix ถูกสร้างขึ้นอย่างไร

UPDATE # 2ตอนนี้พล็อตมีพาหะในพื้นที่ซึ่งแสดงให้เห็นว่าแต่ละองค์ประกอบของการวัดเปลี่ยนตำแหน่งอย่างไร จากพล็อตนี้ฉันจะเห็นได้ว่าองค์ประกอบที่สามของการวัดเปลี่ยนสถานะมากที่สุด

วิธีที่ให้ข้อมูลอย่างมากในการมองเห็นผลของการวัด (สำหรับฉัน) คือการพล็อตสถานะของหุ่นยนต์ (หมายถึงมีวงรีแปรปรวนร่วม) ก่อนและหลังการวัดแต่ละครั้ง จากนั้นนำส่วนประกอบแต่ละส่วนของการวัด (แบริ่งช่วงสำหรับเครื่องหมาย AR) และใช้แยกต่างหากเพื่อรับความรู้สึก

เพื่อทำสิ่งนี้:

$a,b$$\theta$$[0, 2\pi]$

การติดตามความแปรปรวนร่วมของสมมติฐานก่อนหน้านี้สถานะการวัดและสมมติฐานหลังมักจะเพียงพอที่จะค้นหาว่าสมการของ EKF ถูกนำไปใช้อย่างถูกต้องหรือไม่

โชคดีและอย่าอัปเดตคำถามของคุณบ่อยเกินไป กลับมาพร้อมคำถามใหม่แทน

สิ่งที่มักจะทำคือการพล็อตตัวแปรสถานะเมื่อเวลาผ่านไปรวมถึงช่วงเวลา 3-sigma จุดที่ช่วงเวลานี้ลดลงเป็นการอัปเดตซึ่งคุณสามารถใส่คำอธิบายประกอบแหล่งที่มาของการวัดที่เกี่ยวข้อง

นอกจากข้อผิดพลาดในการนำไปใช้งานซึ่งควรตรวจสอบ (ไม่เพียง แต่สมการที่ไม่ถูกต้องเท่านั้น แต่ยังมีสมการที่ไม่เสถียรเช่นกัน) ผลกระทบของการอัพเดตจะได้รับผลกระทบโดยตรงจากความแตกต่างระหว่างสิ่งที่ "คาดหวัง" และ "วัด" ดังนั้นคุณอาจสนใจหาวิธีที่จะเห็นภาพความสมดุลนี้ในแง่ของความก้าวหน้าของเวลาในพล็อตแรก