ตัวกรองคาลมานผูกพัน

ทีมของฉันกำลังสร้างหุ่นยนต์เพื่อนำทางอัตโนมัติในสภาพแวดล้อมกลางแจ้ง เมื่อเร็ว ๆ นี้เราได้รับเซ็นเซอร์ IMU / GPS ในตัวแบบใหม่ซึ่งเห็นได้ชัดว่าบางตัวกรองคาลมานขยายบนชิป มันให้พิทช์ม้วนและหันเหทิศตะวันตกเฉียงเหนือทิศตะวันออกและความเร็วลงและละติจูดและลองจิจูด

อย่างไรก็ตามเรายังมีเอนโค้ดเดอร์บางตัวติดอยู่กับล้อของเราซึ่งให้ความเร็วเชิงเส้นและเชิงมุม ก่อนที่เราจะมีเซ็นเซอร์ IMU / GPS ใหม่นี้เราได้ทำ EKF ของเราเพื่อประเมินสถานะของเราโดยใช้เครื่องเข้ารหัสและเซ็นเซอร์ต้นทุนต่ำอื่น ๆ เราต้องการใช้ตัวกรองบนชิปตัวใหม่ของเซ็นเซอร์นี้ แต่ยังรวมเครื่องเข้ารหัสของเราเข้าด้วยกัน

มีปัญหากับการโยงตัวกรองหรือไม่? สิ่งที่ฉันหมายถึงคือเราจะใช้เอาท์พุทของเซ็นเซอร์ IMU / GPS บนชิป EKF เป็นการอัปเดตเป็น EKF ของเราเองเช่นเดียวกับที่เราใช้ข้อมูลที่อ่านจากตัวเข้ารหัสเป็นการอัปเดตเป็น EKF ของเรา ดูเหมือนจะสมเหตุสมผลสำหรับฉัน แต่ฉันสงสัยว่าสิ่งใดที่ควรทำในกรณีนี้

คุณสามารถใช้ INS / GPS เป็นอัปเดตสำหรับเอาต์พุตของ EKF แรกของคุณ อันที่จริงแล้วไม่ใช่การผูกมัด แต่เพียงแค่ปรับการประมาณตามข้อมูลเพิ่มเติมจาก INS / GPS

สมมติว่าเรามีฟังก์ชั่นต่อไปนี้:

$x_{t+1|t}$ , = EKF_PREDICT ( , , ) สำหรับปัจจัยการผลิตเป็นของรัฐแปรปรวน , และปัจจัยการผลิตการควบคุม (ประเมินโดย odometry) u_t$P_{t+1|t}$$x_t$$P_t$$u_t$$x$$P$$u_t$

และ

$x_{t+1|t+1}$ , = EKF_UPDATE ( , , ) $P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}$

ประมาณการจากเซ็นเซอร์เป็น1} เรามีสิ่งที่ชอบ:$\hat{x}_{t+1}$

$\hat{x}^{gps}_{t+1} = f(GPS)$

$\hat{x}^{map}_{t+1} = f(map)$

$\hat{x}^{ins}_{t+1} = f(INS)$

ฯลฯ สำหรับวิธีอื่น ๆ ทั้งหมดในการประเมินสถานะของหุ่นยนต์ ดังนั้นการเรียกใช้ฟังก์ชัน EKF_UPDATE สำหรับเซ็นเซอร์ทั้งหมดนั้นดีพอ

วงของคุณจะเป็นแบบนี้:

ตลอดเวลา$t$

• ให้เป็นค่าประมาณ odometry / kinematic ปัจจุบันของการโพสต์และเป็นเสียงในการประมาณนั้น$u_t$$R_u$

• $x_{t+1|t}$ , = EKF_PREDICT ( , , , )$P_{t+1|t}$$x_t$$P_t$$u_t$$R_u$

• สำหรับเซ็นเซอร์ทั้งหมด ,$S$

  • ให้เป็นค่าประมาณของท่าทางจากเซ็นเซอร์นั้นและเป็นเสียงรบกวนจากการประมาณนั้น$\hat{x}^{S}_{t+1}$$R_{S}$

  • $x_{t+1|t+1}$ , = EKF_UPDATE ( , , ) $P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}, R_S$

  • สิ้นสุดสำหรับ

• สิ้นสุดสำหรับ

ข้อควรทราบบางประการคือ:

• เนื่องจากเราใช้ EKF จึงไม่มีการรับประกันว่าการประมาณการจะเป็นอิสระจากการสั่งซื้อการอัปเดต นั่นคือถ้าคุณทำ INS กับ GPS การประมาณผลลัพธ์อาจแตกต่างจากถ้าคุณอัปเดตด้วย GPS แล้ว INS นี่ไม่ใช่เรื่องปกติ แต่ตัวกรองจะต้องปรับแต่งอย่างมีนัยสำคัญ

• โปรดระวัง INS ของคุณมีอคติและการเบี่ยงเบนซึ่งอาจส่งผลต่อความน่าเชื่อถือในระยะยาวของคุณ GPS ช่วยคุณได้มากที่นี่ วรรณกรรมส่วนใหญ่ประเมินความเอนเอียงและการเบี่ยงเบนพร้อมกันในเวลาเดียวกัน