智能機(jī)器人與運(yùn)動(dòng)控制系統(tǒng)20.ROS的EKF定位與SLAM實(shí)現(xiàn)EKF定位程序框圖學(xué)習(xí)目標(biāo)使用rviz的teleop面板發(fā)出控制信號(hào)，控制unicycle模型離散化unicycle運(yùn)動(dòng)學(xué)方程編寫uni_controller.py，實(shí)現(xiàn)控制檢驗(yàn)觀測部分代碼是否正常學(xué)會(huì)運(yùn)行pyc文件學(xué)會(huì)rqt_image_view查看圖片EKF定位實(shí)驗(yàn)推導(dǎo)線性化的Jacobi矩陣推導(dǎo)ekf過程：預(yù)測+更新編寫ekf_localization.py，實(shí)現(xiàn)定位Unicycle非線性控制非線性顏色觀測EKF定位Unicycle非線性控制Unicycle控制離散化unicycle運(yùn)動(dòng)學(xué)方程編寫uni_controller.py提示：Gazebo世界坐標(biāo)系：x（紅軸），y（綠軸），z（藍(lán)軸）Theta范圍（-pi,pi），其中x軸正向?yàn)榱愣?，單位為rad改變model角度主要修改：ModelState.orientation中的zw（orientation中的xyzw是四元數(shù)的表達(dá)，需要考慮歐拉角到四元數(shù)的計(jì)算公式）Q與delta_t有關(guān)簡單離散化Unicycle控制預(yù)計(jì)效果非線性顏色觀測非線性顏色觀測9一、檢查模型名稱和話題名稱rostopiclist檢查圖像話題是否為以/unicycle/camera1/image_raw開頭如果不是，修改model.sdf中與圖像話題相關(guān)的設(shè)置非線性顏色觀測10二、運(yùn)行觀測部分代碼，檢查輸出是否正常$pythonmeas_publisher.pyc介紹：.pyc文件是由.py文件經(jīng)過編譯后生成的字節(jié)碼文件，其加載速度相對于之前的.py文件有所提高

主要檢查：是否能同時(shí)觀測到四個(gè)不同顏色的圓柱體（看輸出）（這里需要十分關(guān)注顏色的順序，與后面輸出數(shù)據(jù)含義對應(yīng)相關(guān)）介紹：.pyc文件需要在.pyc文件所在路徑下運(yùn)行上述命令；而不能使用rosrun終端輸入rqt_image_view可以查看當(dāng)前獲得的圖片（報(bào)錯(cuò)不影響）非線性顏色觀測11三、觀測部分輸出詳細(xì)說明運(yùn)行meas_publisher.pyc文件會(huì)在話題：/unicycle/measurement下發(fā)送數(shù)據(jù)格式為Float32MultiArray的數(shù)據(jù)；Float32MultiArray：一維數(shù)組，包含12個(gè)數(shù)，表明對于四個(gè)圓柱體的觀測信息即：是否觀測到（0/1）、距離、朝向（rad）（重復(fù)4次；顏色順序與終端出現(xiàn)順序?qū)?yīng)）1.四個(gè)都觀測到：2.藍(lán)色和黃色觀測到；紅色和綠色未觀測到：非線性顏色觀測12四、預(yù)計(jì)效果EKF定位EKF定位一、推導(dǎo)線性化的Jacobi矩陣二、推導(dǎo)ekf過程：預(yù)測+更新整體邏輯與kf類似；（kf的A用ekf的Jacobi矩陣F替代；kf的C用ekf的Jacobi矩陣H替代）在更新的時(shí)候，對于每一個(gè)路標(biāo)點(diǎn)都有如下的觀測方程；