{"id":793,"date":"2023-04-02T17:52:18","date_gmt":"2023-04-02T09:52:18","guid":{"rendered":"http:\/\/www.autouav.net.cn\/?p=793"},"modified":"2023-06-15T16:29:56","modified_gmt":"2023-06-15T08:29:56","slug":"ardupilot-%e5%8d%a1%e5%b0%94%e6%9b%bc%e6%bb%a4%e6%b3%a2%e7%ae%97%e6%b3%95","status":"publish","type":"post","link":"https:\/\/www.autouav.net.cn\/?p=793","title":{"rendered":"ArduPilot \u5361\u5c14\u66fc\u6ee4\u6ce2\u7b97\u6cd5"},"content":{"rendered":"<p>ArduPilot\u662f\u4e00\u4e2a\u5f00\u6e90\u7684\u98de\u884c\u63a7\u5236\u7cfb\u7edf\uff0c\u5b83\u652f\u6301\u591a\u79cd\u65e0\u4eba\u673a\u548c\u81ea\u4e3b\u8f66\u8f86\u3002\u4f5c\u4e3a\u4e00\u4e2a\u5f00\u6e90\u9879\u76ee\uff0c\u5176\u4ee3\u7801\u53ef\u4ee5\u5728 Github \u4e0a\u516c\u5f00\u8bbf\u95ee\u3002<\/p>\n<p>ArduPilot\u7684\u5361\u5c14\u66fc\u6ee4\u6ce2\u4ee3\u7801\u4f4d\u4e8elibraries\/AP_NavEKF2\u76ee\u5f55\u4e2d\u3002NavEKF2\u4ee3\u8868Navigation EKF second generation\uff0c\u5373\u5bfc\u822a\u6269\u5c55\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u7b2c\u4e8c\u4ee3\u3002<\/p>\n<p>\u5728\u8fd9\u4e2a\u76ee\u5f55\u4e2d\uff0c\u6700\u91cd\u8981\u7684\u4ee3\u7801\u6587\u4ef6\u662fEKF.cpp\uff0c\u5b83\u5305\u542b\u4e86\u6574\u4e2aEKF\u7b97\u6cd5\u7684\u5b9e\u73b0\u3002\u8be5\u6587\u4ef6\u5b9e\u73b0\u4e86\u89e3\u6790\u9640\u87ba\u4eea\u3001\u52a0\u901f\u5ea6\u8ba1\u3001\u78c1\u529b\u8ba1\u3001GPS\u7b49\u4f20\u611f\u5668\u6570\u636e\u7684\u8fc7\u7a0b\uff0c\u5e76\u4f7f\u7528\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u6765\u4f30\u8ba1\u7cfb\u7edf\u7684\u72b6\u6001\u3002<\/p>\n<p>\u9664\u4e86EKF.cpp\u4e4b\u5916\uff0c\u8fd8\u6709\u5176\u4ed6\u4e00\u4e9b\u6587\u4ef6\u4e5f\u5305\u62ec\u4e00\u4e9b\u4e0e\u5361\u5c14\u66fc\u6ee4\u6ce2\u76f8\u5173\u7684\u4ee3\u7801\uff0c\u4f8b\u5982\uff1a<\/p>\n<ul>\n<li>AP_NavEKF2.h\uff1a\u5b9a\u4e49\u4e86\u5bfc\u822aEK\u7684\u914d\u7f6e\u9009\u9879\u548c\u5404\u79cd\u6570\u636e\u7ed3\u6784\u3002<\/li>\n<li>AP_NavEKF2_core.cpp: \u5b9e\u73b0\u4e86\u5bfc\u822aEK\u7684\u57fa\u672c\u529f\u80fd\uff0c\u4f8b\u5982\u66f4\u65b0\u72b6\u6001\u548c\u8ba1\u7b97\u822a\u5411\u3002<\/li>\n<li>AP_NavEKF2_core.h\uff1a\u5b9a\u4e49\u4e86\u5bfc\u822aEK\u7684\u6838\u5fc3\u529f\u80fd\u63a5\u53e3\u3002<\/li>\n<\/ul>\n<p>\u5728ArduPilot\u4e2d\uff0c\u5361\u5c14\u66fc\u6ee4\u6ce2\u4f7f\u7528\u4e86\u4e00\u79cd\u79f0\u4e3aEKF\uff08Extended Kalman Filter\uff09\u7684\u6539\u8fdb\u578b\u7b97\u6cd5\uff0c\u7528\u4e8e\u540c\u65f6\u5904\u7406\u591a\u9879\u4f20\u611f\u5668\u7684\u6570\u636e\u3002<\/p>\n<p>\u8be5\u7b97\u6cd5\u7684\u4e2d\u4f53\u6846\u67b6\u5982\u4e0b\uff1a<\/p>\n<ol>\n<li>\u9884\u6d4b\u72b6\u6001<\/li>\n<\/ol>\n<p>\u5728\u63a7\u5236\u7cfb\u7edf\u4e2d\uff0c\u6211\u4eec\u9700\u8981\u5bf9\u65e0\u4eba\u673a\u7684\u4f4d\u7f6e\u3001\u901f\u5ea6\u3001\u89d2\u5ea6\u7b49\u72b6\u6001\u8fdb\u884c\u9884\u6d4b\u3002\u8fd9\u53ef\u4ee5\u901a\u8fc7\u5229\u7528\u7269\u7406\u65b9\u7a0b\u4e0e\u4e0a\u4e00\u65f6\u523b\u7684\u72b6\u6001\u8fdb\u884c\u8ba1\u7b97\u6765\u5b9e\u73b0\u3002\u5728ArduPilot\u4e2d\uff0c\u9884\u6d4b\u72b6\u6001\u7684\u8fc7\u7a0b\u7528\u4e00\u4e2a\u51fd\u6570<code>EKFFusion::CalculatePrediction()<\/code>\u5b9e\u73b0\u3002\u8be5\u51fd\u6570\u4f1a\u6839\u636e\u7cfb\u7edf\u52a8\u529b\u5b66\u6a21\u578b\u5c06\u4e0a\u6b21\u7684\u4f30\u8ba1\u72b6\u6001\u503c\u548c\u53c2\u6570\u8f93\u5165\uff0c\u8ba1\u7b97\u51fa\u5f53\u524d\u65f6\u95f4\u6b65\u4f30\u8ba1\u51fa\u6765\u7684\u72b6\u6001\u66f4\u65b0\u503c\uff0c\u5b58\u50a8\u5230kalmanStatePred\u7ed3\u6784\u4f53\u4e2d\u3002<\/p>\n<ol start=\"2\">\n<li>\u4f20\u611f\u5668\u6570\u636e\u878d\u5408<\/li>\n<\/ol>\n<p>\u5728\u63a7\u5236\u7cfb\u7edf\u4e2d\uff0c\u6211\u4eec\u901a\u5e38\u4f1a\u4f7f\u7528\u591a\u4e2a\u4f20\u611f\u5668\u8bfb\u53d6\u4e0d\u540c\u7684\u72b6\u6001\u91cf\uff0c\u5e76\u5c06\u5b83\u4eec\u5408\u5e76\u6210\u4e00\u4e2a\u6709\u5173\u76ee\u6807\u7684\u66f4\u5b8c\u6574\u7684\u72b6\u6001\u4f30\u8ba1\u3002\u5728ArduPilot\u4e2d\uff0c\u4f7f\u7528\u4f20\u611f\u5668\u6570\u636e\u96c6\u7684\u8d21\u732e\u6765\u4f30\u8ba1\u65b0\u7684\u822a\u7a7a\u4f4d\u7f6e\u89e3\u548c\u80fd\u591f\u4f7f\u5f97\u6240\u6709\u4f20\u611f\u5668\u6570\u636e\u70b9\u5185\u90e8\u6700\u5c0f\u5316\u8bef\u5dee\u504f\u5dee\u6270\u52a8\u7684\u72b6\u6001\u65b9\u5dee\/\u534f\u65b9\u5dee\u77e9\u9635\u3002<\/p>\n<p>\u8be5\u7b97\u6cd5\u7684\u8f93\u5165\u662f\u59ff\u6001\u3001\u4f4d\u7f6e\u548c\u901f\u5ea6\u6d4b\u91cf\u53ca\u5176\u5bf9\u5e94\u7684\u72b6\u6001\u566a\u58f0\u6a21\u578b\u3002\u5728\u9884\u6d4b\u72b6\u6001\u540e\uff0c\u901a\u8fc7\u5c06\u5f97\u5230\u7684\u4f20\u611f\u5668\u6570\u636e\u4e0e\u521a\u624d\u7684EKF\u9884\u6d4b\u8fdb\u884c\u6bd4\u8f83\uff0c\u5e76\u4f7f\u7528Kalman\u589e\u76ca\u4fee\u6b63\u9884\u6d4b\u6765\u6821\u51c6\u4e00\u90e8\u5206\u6216\u6240\u6709\u7684\u72b6\u6001\u53d8\u91cf\u3002\u5982\u679c\u4f20\u611f\u5668\u6570\u636e\u4ee5\u5185\u90e8\u7684\u6807\u51c6\u8fdb\u884c\u6821\u51c6\uff0c\u5b83\u4eec\u7684\u6700\u4f73\u8d21\u732e\u5c06\u88ab\u5408\u5e76\u4e3a\u4e00\u4e2a\u6700\u7ec8\u4f30\u8ba1\u503c\u3002\u5728ArduPilot\u4e2d\uff0c\u4f20\u611f\u5668\u6570\u636e\u878d\u5408\u8fc7\u7a0b\u7528\u4e00\u4e2a\u51fd\u6570<code>EKFFusion::Fuse()<\/code>\u5b9e\u73b0\u3002<\/p>\n<ol start=\"3\">\n<li>\u8bef\u5dee\u8865\u507f<\/li>\n<\/ol>\n<p>\u5f53\u6211\u4eec\u4f7f\u7528\u6570\u636e\u8fdb\u884c\u6821\u51c6\u65f6\uff0c\u53ef\u80fd\u4f1a\u53d1\u73b0\u6211\u4eec\u6240\u4f7f\u7528\u7684\u6570\u636e\u6709\u4e00\u5b9a\u7684\u8bef\u5dee\u548c\u504f\u5dee\u3002\u56e0\u6b64\uff0c\u5728\u6570\u636e\u878d\u5408\u4e4b\u540e\uff0c\u6211\u4eec\u9700\u8981\u8fdb\u4e00\u6b65\u5730\u8fdb\u884c\u8bef\u5dee\u8865\u507f\u3002\u901a\u5e38\uff0c\u8bef\u5dee\u8865\u507f\u662f\u6d88\u9664\u4f20\u611f\u5668\u7578\u53d8\u548c\u7cfb\u7edf\u504f\u5dee\u7684\u8fc7\u7a0b\u3002\u5728ArduPilot\u4e2d\uff0c\u8bef\u5dee\u8865\u507f\u8fc7\u7a0b\u7531<code>EKFFusion::CorrectStates()<\/code>\u51fd\u6570\u5b9e\u73b0\u3002\u8be5\u51fd\u6570\u5c06\u6821\u51c6\u8fc7\u7684\u4f20\u611f\u5668\u6570\u636e\u6295\u5f71\u56de\u539f\u59cb\u72b6\u6001\u53d8\u91cf\u4e0a\uff0c\u5e76\u8ba1\u7b97\u8bef\u5dee\u540e\uff0c\u518d\u6b21\u66f4\u65b0kalmanState\u7ed3\u6784\u4f53\u6765\u5b8c\u6210\u5f53\u524d\u98de\u884c\u95f4\u9694\uff08delta_time\uff09\u4e0b\u673a\u4f53\u89d2\u5ea6\uff0c\u901f\u5ea6\u53ca\u4f4d\u7f6e\u7b49\u72b6\u6001\u7684\u5361\u5c14\u66fc\u6ee4\u6ce2\u66f4\u65b0\u3002<\/p>\n<ol start=\"4\">\n<li>\u8f93\u51fa<\/li>\n<\/ol>\n<p>\u5f53\u4ee5\u4e0a\u6b65\u9aa4\u5b8c\u6210\u540e\uff0c\u6211\u4eec\u5c31\u53ef\u4ee5\u4f7f\u7528EKF\u5728\u52a8\u6001\u8fd0\u884c\u7684\u7cfb\u7edf\u4e0b\u5bf9\u65e0\u4eba\u673a\u8fdb\u884c\u59ff\u6001\u3001\u4f4d\u7f6e\u548c\u901f\u5ea6\u4f30\u8ba1\u4e86\u3002 ArduPilot\u5c06\u7ed3\u679c\u8f93\u51fa\u5230\u7cfb\u7edf\u7684\u6307\u4ee4\u961f\u5217\u4e2d\uff0c\u901a\u8fc7\u5148\u524d\u5b9a\u4e49\u597d\u7684\u7cfb\u7edf\u547d\u4ee4\u5b8c\u6210\u63a7\u5236\u7b97\u6cd5\uff0c\u5b9e\u73b0\u4e86\u81ea\u4e3b\u98de\u884c\u3002<\/p>\n<p>\u7efc\u4e0a\u6240\u8ff0\uff0cArduPilot\u4e2d\u5361\u5c14\u66fc\u6ee4\u6ce2\u7684\u57fa\u672c\u6846\u67b6\u5305\u62ec\u9884\u6d4b\u72b6\u6001\u3001\u4f20\u611f\u5668\u6570\u636e\u878d\u5408\u3001\u8bef\u5dee\u8865\u507f\u7b49\u6b65\u9aa4\u3002\u5176\u4e2d\u9884\u6d4b\u72b6\u6001\u548c\u4f20\u611f\u5668\u6570\u636e\u878d\u5408\u662f\u6574\u4e2a\u7b97\u6cd5\u6700\u4e3a\u6838\u5fc3\u7684\u90e8\u5206\uff0c\u5728\u5e7f\u6cdb\u5e94\u7528\u591a\u9879\u4f20\u611f\u5668\u7684\u540c\u65f6\uff0c\u53ef\u4ee5\u5927\u5927\u63d0\u9ad8\u98de\u884c\u59ff\u6001\u4f30\u8ba1\u53ca\u5b9a\u4f4d\u7684\u7cbe\u5ea6\u548c\u53ef\u9760\u6027 \u3002<\/p>\n","protected":false},"excerpt":{"rendered":"<p>ArduPilot\u662f\u4e00\u4e2a\u5f00\u6e90\u7684\u98de\u884c\u63a7\u5236\u7cfb\u7edf\uff0c\u5b83\u652f\u6301\u591a\u79cd\u65e0\u4eba\u673a\u548c\u81ea\u4e3b\u8f66\u8f86\u3002\u4f5c\u4e3a\u4e00\u4e2a\u5f00\u6e90\u9879\u76ee\uff0c\u5176\u4ee3\u7801\u53ef\u4ee5\u5728 Github \u4e0a\u516c\u5f00\u8bbf\u95ee\u3002 ArduPilot\u7684\u5361\u5c14\u66fc\u6ee4\u6ce2\u4ee3\u7801\u4f4d\u4e8elibraries\/AP_Na&#8230; <\/p>\n","protected":false},"author":1,"featured_media":795,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[1,25],"tags":[],"class_list":["post-793","post","type-post","status-publish","format-standard","has-post-thumbnail","hentry","category-uav","category-25"],"_links":{"self":[{"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/posts\/793","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=793"}],"version-history":[{"count":1,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/posts\/793\/revisions"}],"predecessor-version":[{"id":794,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/posts\/793\/revisions\/794"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/media\/795"}],"wp:attachment":[{"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=793"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=793"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=793"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}