{"id":796,"date":"2023-04-02T17:59:14","date_gmt":"2023-04-02T09:59:14","guid":{"rendered":"http:\/\/www.autouav.net.cn\/?p=796"},"modified":"2023-04-02T23:22:23","modified_gmt":"2023-04-02T15:22:23","slug":"c%e8%af%ad%e8%a8%80-%e5%8d%a1%e5%b0%94%e6%9b%bc%e6%bb%a4%e6%b3%a2%ef%bc%88kalman-filter%ef%bc%89","status":"publish","type":"post","link":"https:\/\/www.autouav.net.cn\/?p=796","title":{"rendered":"C\u8bed\u8a00 \u5361\u5c14\u66fc\u6ee4\u6ce2\uff08Kalman Filter\uff09"},"content":{"rendered":"<p>\u5361\u5c14\u66fc\u6ee4\u6ce2\uff08Kalman Filter\uff09\u7b97\u6cd5\u662f\u4e00\u79cd\u5229\u7528\u7ebf\u6027\u5361\u5c14\u66fc\u6ee4\u6ce2\u65b9\u7a0b\u6765\u9884\u6d4b\u548c\u4f30\u8ba1\u7cfb\u7edf\u72b6\u6001\u7684\u8fc7\u7a0b\u63a7\u5236\u65b9\u6cd5\u3002\u4e0b\u9762\u6f14\u793a\u5982\u4f55\u57fa\u4e8eC\u8bed\u8a00\u7f16\u5199\u4e00\u4e2a\u7b80\u5355\u7684\u4e00\u7ef4\u5361\u5c14\u66fc\u6ee4\u6ce2\u7b97\u6cd5\u3002<\/p>\n<p>\u9996\u5148\uff0c\u5b9a\u4e49\u4e00\u4e2a\u7ed3\u6784\u4f53\u6765\u50a8\u5b58\u5361\u5c14\u66fc\u6ee4\u6ce2\u7684\u53c2\u6570\u548c\u72b6\u6001\uff1a<\/p>\n<p>typedef struct {<br \/>\nfloat q; \/\/ \u8fc7\u7a0b\u566a\u58f0\u65b9\u5dee<br \/>\nfloat r; \/\/ \u6d4b\u91cf\u566a\u58f0\u65b9\u5dee<br \/>\nfloat x; \/\/ \u72b6\u6001\u53d8\u91cf\u503c\uff08\u521d\u59cb\u503c\uff09<br \/>\nfloat p; \/\/ \u5bf9\u89d2\u7ebf\u4e0a\u7684\u8bef\u5dee\u534f\u65b9\u5dee\uff08\u521d\u59cb\u503c\uff09<br \/>\nfloat k; \/\/ \u5361\u5c14\u66fc\u589e\u76ca<br \/>\n} kalman_filter;<\/p>\n<p>\u53c2\u6570\u00a0<code>q<\/code>\u00a0\u8868\u793a\u8fc7\u7a0b\u566a\u58f0\u65b9\u5dee\uff0c\u5373\u6a21\u578b\u4e2d\u672a\u8003\u8651\u5230\u7684\u8bef\u5dee\u7684\u65b9\u5dee\uff1b\u53c2\u6570\u00a0<code>r<\/code>\u00a0\u8868\u793a\u6d4b\u91cf\u566a\u58f0\u65b9\u5dee\uff0c\u5373\u6d4b\u91cf\u7ed3\u679c\u7684\u8bef\u5dee\u7684\u65b9\u5dee\uff1b<code>x<\/code>\u00a0\u662f\u72b6\u6001\u53d8\u91cf\uff0c\u8868\u793a\u6211\u4eec\u8981\u4f30\u8ba1\u7684\u503c\uff1b<code>p<\/code>\u00a0\u662f\u5bf9\u89d2\u7ebf\u4e0a\u7684\u8bef\u5dee\u534f\u65b9\u5dee\uff0c\u53cd\u6620\u6211\u4eec\u5f53\u524d\u5bf9\u4f30\u8ba1\u503c\u4e0d\u786e\u5b9a\u5ea6\u7684\u8bc4\u4f30\uff1b<code>k<\/code>\u00a0\u662f\u5361\u5c14\u66fc\u589e\u76ca\uff0c\u8868\u793a\u6211\u4eec\u6839\u636e\u5df2\u77e5\u6570\u636e\u8ba1\u7b97\u5f97\u5230\u7684\u65b0\u7684\u72b6\u6001\u53d8\u91cf\u3001\u9519\u8bef\u534f\u65b9\u5dee\u548c\u672a\u77e5\u6570\u636e\u4e4b\u95f4\u7684\u5173\u7cfb\u3002<\/p>\n<p>\u63a5\u4e0b\u6765\uff0c\u5b9a\u4e49\u4e00\u4e2a\u51fd\u6570\u6765\u8ba1\u7b97\u65b0\u7684\u5361\u5c14\u66fc\u6ee4\u6ce2\u72b6\u6001\uff0c\u5e76\u8fd4\u56de\u6211\u4eec\u4f30\u8ba1\u51fa\u7684\u503c\uff1a<\/p>\n<p>float kalman_update(kalman_filter* filter, float data) {<br \/>\n\/\/ \u9884\u6d4b\u9636\u6bb5\uff08\u65e0\u63a7\u5236\u8f93\u5165\uff09<br \/>\nfilter-&gt;x = filter-&gt;x; \/\/ \u72b6\u6001\u65b9\u7a0b\uff1aX_k = F_k * X_k-1 + B_k * U_k<br \/>\nfilter-&gt;p = filter-&gt;p + filter-&gt;q; \/\/ \u8bef\u5dee\u534f\u65b9\u5dee\u66f4\u65b0\uff1aP_k = F_k * P_k-1 * F_k^T + Q_k<\/p>\n<p>\/\/ \u66f4\u65b0\u9636\u6bb5\uff08\u6709\u6d4b\u91cf\u8f93\u5165\uff09<br \/>\nfilter-&gt;k = filter-&gt;p \/ (filter-&gt;p + filter-&gt;r); \/\/ \u5361\u5c14\u66fc\u589e\u76ca\u8ba1\u7b97\uff1aK_k = P_k * H_k^T * (H_k*P_k*H_k^T + R_k)^-1<br \/>\nfilter-&gt;x = filter-&gt;x + filter-&gt;k * (data &#8211; filter-&gt;x); \/\/ \u72b6\u6001\u66f4\u65b0\uff1aX_k = X_k + K_k * (Z_k &#8211; H_k * X_k)<br \/>\nfilter-&gt;p = (1 &#8211; filter-&gt;k) * filter-&gt;p; \/\/ \u8bef\u5dee\u534f\u65b9\u5dee\u66f4\u65b0\uff1aP_k = (I &#8211; K_k * H_k) * P_k<\/p>\n<p>\/\/ \u8fd4\u56de\u8fc7\u6e21\u503c<br \/>\nreturn filter-&gt;x;<br \/>\n}<\/p>\n<p>\u4e0a\u8ff0\u4ee3\u7801\u4e2d\uff0c<code>data<\/code>\u00a0\u662f\u6211\u4eec\u8981\u8fdb\u884c\u5361\u5c14\u66fc\u6ee4\u6ce2\u7684\u6570\u636e\uff0c\u901a\u8fc7\u8c03\u7528\u00a0<code>kalman_update<\/code>\u00a0\u51fd\u6570\uff0c\u53ef\u4ee5\u5c06\u5176\u8ba1\u7b97\u4e3a\u57fa\u4e8e\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u5f53\u524d\u72b6\u6001\u7684\u9884\u6d4b\u3002\u5728\u4e0a\u8ff0\u4ee3\u7801\u4e2d\uff0c\u503c\u5f97\u6ce8\u610f\u7684\u662f\uff1a<\/p>\n<ul>\n<li><code>filter-&gt;x = filter-&gt;x<\/code>\u00a0\u4e0e\u00a0<code>filter-&gt;p = filter-&gt;p + filter-&gt;q<\/code>\u00a0\u8868\u793a\u9884\u6d4b\u9636\u6bb5\u7684\u72b6\u6001\u4ee5\u53ca\u8bef\u5dee\u534f\u65b9\u5dee\uff0c\u5e76\u4e0d\u4f1a\u6839\u636e\u6d4b\u91cf\u6570\u636e\u8fdb\u884c\u66f4\u65b0\u3002<\/li>\n<li><code>filter-&gt;k = filter-&gt;p \/ (filter-&gt;p + filter-&gt;r)<\/code>\u00a0\u8ba1\u7b97\u4e86\u65b0\u7684\u5361\u5c14\u66fc\u589e\u76ca\u3002\u5206\u6bcd\u4e3a\u8fc7\u6e21\u53d8\u91cf\u548c\u6d4b\u91cf\u566a\u58f0\u65b9\u5dee\u4e4b\u548c\uff0c\u5206\u5b50\u5219\u662f\u6211\u4eec\u5728\u9884\u6d4b\u671f\u95f4\u8ba1\u7b97\u5f97\u51fa\u7684\u534f\u65b9\u5dee\u3002<\/li>\n<li><code>filter-&gt;x + filter-&gt;k * (data - filter-&gt;x)<\/code>\u00a0\u5219\u662f\u5728\u66f4\u65b0\u9636\u6bb5\uff08\u6709\u6d4b\u91cf\u8f93\u5165\uff09\u8fdb\u884c\u7684\u72b6\u6001\u66f4\u65b0\u3002<\/li>\n<li><code>filter-&gt;p = (1 - filter-&gt;k) * filter-&gt;p<\/code>\u00a0\u5219\u662f\u5728\u66f4\u65b0\u9636\u6bb5\uff08\u6709\u6d4b\u91cf\u8f93\u5165\uff09\u8fdb\u884c\u7684\u8bef\u5dee\u534f\u65b9\u5dee\u66f4\u65b0\u3002<\/li>\n<\/ul>\n<p>\u6700\u540e\uff0c\u4f7f\u7528\u4ee5\u4e0b\u4ee3\u7801\u8c03\u7528\u4e0a\u8ff0\u51fd\u6570\uff0c\u5c1d\u8bd5\u5bf9\u4e00\u7ec4\u6570\u636e\u8fdb\u884c\u5361\u5c14\u66fc\u6ee4\u6ce2\uff0c\u5e76\u8f93\u51fa\u7ed3\u679c\uff1a<\/p>\n<p>int main() {<br \/>\n\/\/ \u521d\u59cb\u5316\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668<br \/>\nkalman_filter filter;<br \/>\nfilter.q = 0.01f; \/\/ \u8fc7\u7a0b\u566a\u58f0\u65b9\u5dee\uff0c\u53ef\u4ee5\u6839\u636e\u5177\u4f53\u5e94\u7528\u9700\u6c42\u8fdb\u884c\u8c03\u6574<br \/>\nfilter.r = 0.1f; \/\/ \u6d4b\u91cf\u566a\u58f0\u65b9\u5dee\uff0c\u53ef\u4ee5\u6839\u636e\u5177\u4f53\u5e94\u7528\u9700\u6c42\u8fdb\u884c\u8c03\u6574<br \/>\nfilter.x = 10.0f; \/\/ \u72b6\u6001\u53d8\u91cf\u521d\u59cb\u503c<br \/>\nfilter.p = 1.0f; \/\/ \u8bef\u5dee\u534f\u65b9\u5dee\u521d\u59cb\u503c<\/p>\n<p>float data[10] = {10.5f, 11.2f, 10.8f, 9.6f, 12.3f, 10.6f, 11.8f, 9.5f, 12.5f, 13.0f};<br \/>\nfor(int i=0; i&lt;10; i++) {<br \/>\nprintf(&#8220;%.2f\\n&#8221;, kalman_update(&amp;filter, data[i]));<br \/>\n}<\/p>\n<p>}<\/p>\n<p>\u4e0a\u8ff0\u4ee3\u7801\u4e2d\uff0c\u9996\u5148\u521d\u59cb\u5316\u4e00\u4e2a\u00a0<code>kalman_filter<\/code>\u00a0\u7c7b\u578b\u7684\u6ee4\u6ce2\u5668\uff0c\u5e76\u5206\u522b\u8bbe\u7f6e\u8fc7\u7a0b\u566a\u58f0\u548c\u6d4b\u91cf\u566a\u58f0\u65b9\u5dee\u3002\u63a5\u7740\uff0c\u6211\u4eec\u7528\u4e00\u4e2a\u957f\u5ea6\u4e3a10\u7684\u6570\u7ec4\u6765\u5b58\u50a8\u5f85\u6ee4\u6ce2\u7684\u6570\u636e\uff0c\u5e76\u5728\u4e00\u4e2a\u5faa\u73af\u4e2d\u4f9d\u6b21\u8c03\u7528\u00a0<code>kalman_update<\/code>\u00a0\u51fd\u6570\u6765\u5bf9\u6bcf\u4e2a\u6570\u636e\u8fdb\u884c\u6ee4\u6ce2\u5904\u7406\uff0c\u6700\u540e\u8f93\u51fa\u7ed3\u679c\u3002<\/p>\n<p>\u9700\u8981\u6ce8\u610f\u7684\u662f\uff0c\u8fd9\u53ea\u662f\u4e00\u7ef4\u7684\u5361\u5c14\u66fc\u6ee4\u6ce2\u7b97\u6cd5\uff0c\u5982\u679c\u4f60\u9700\u8981\u8fdb\u884c\u66f4\u9ad8\u7ef4\u5ea6\u6216\u8005\u66f4\u52a0\u590d\u6742\u7684\u6ee4\u6ce2\uff0c\u90a3\u4e48\u4f60\u9700\u8981\u8fdb\u884c\u8fdb\u4e00\u6b65\u7684\u5b66\u4e60\u3001\u7814\u7a76\u5e76\u4fee\u6539\u4ee5\u4e0a\u4ee3\u7801\u3002<\/p>\n","protected":false},"excerpt":{"rendered":"<p>\u5361\u5c14\u66fc\u6ee4\u6ce2\uff08Kalman Filter\uff09\u7b97\u6cd5\u662f\u4e00\u79cd\u5229\u7528\u7ebf\u6027\u5361\u5c14\u66fc\u6ee4\u6ce2\u65b9\u7a0b\u6765\u9884\u6d4b\u548c\u4f30\u8ba1\u7cfb\u7edf\u72b6\u6001\u7684\u8fc7\u7a0b\u63a7\u5236\u65b9\u6cd5\u3002\u4e0b\u9762\u6f14\u793a\u5982\u4f55\u57fa\u4e8eC\u8bed\u8a00\u7f16\u5199\u4e00\u4e2a\u7b80\u5355\u7684\u4e00\u7ef4\u5361\u5c14\u66fc\u6ee4\u6ce2\u7b97\u6cd5\u3002 \u9996\u5148\uff0c\u5b9a\u4e49\u4e00\u4e2a\u7ed3\u6784\u4f53\u6765\u50a8\u5b58\u5361\u5c14\u66fc\u6ee4&#8230; <\/p>\n","protected":false},"author":1,"featured_media":800,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[25],"tags":[],"class_list":["post-796","post","type-post","status-publish","format-standard","has-post-thumbnail","hentry","category-25"],"_links":{"self":[{"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/posts\/796","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=796"}],"version-history":[{"count":1,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/posts\/796\/revisions"}],"predecessor-version":[{"id":797,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/posts\/796\/revisions\/797"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=\/wp\/v2\/media\/800"}],"wp:attachment":[{"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=796"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=796"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.autouav.net.cn\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=796"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}