:通用觀測更新內(nèi)核 `measurementUpdate())
PX4 EKF2 源碼解析(九):通用觀測更新內(nèi)核measurementUpdate()摘要不同傳感器具有不同觀測模型,但最終都要完成協(xié)方差和狀態(tài)更新。本文以標量觀測為對象,解析創(chuàng)新方差、卡爾曼增益、狀態(tài)抑制、協(xié)方差健康檢查和fuse(),建立各*_fusion.cpp的統(tǒng)一閱讀模板。1. 標量觀測更新對單個觀測分量:ν=h(x)?z,S=HPHT+R,K=PHT/S \nu=h(x)-z, \quad S=HPH^T+R, \quad K=PH^T/Sν=