{"id":590,"date":"2015-01-27T20:45:06","date_gmt":"2015-01-27T12:45:06","guid":{"rendered":"http:\/\/blog.xuhao1.me\/?p=590"},"modified":"2015-01-27T20:46:54","modified_gmt":"2015-01-27T12:46:54","slug":"probabilistic-robotics-recursive-state-estimation%e5%ad%a6%e4%b9%a0","status":"publish","type":"post","link":"http:\/\/blog.xuhao1.me\/?p=590","title":{"rendered":"Probabilistic Robotics\u5b66\u4e60\u7b14\u8bb0: Bayes Filter \u57fa\u672c\u6982\u5ff5"},"content":{"rendered":"

\u5047\u671f\u5b66\u4e60Probabilistic Robotics\u7684\u4e00\u4e9b\u7b14\u8bb0
\n\u4f7f\u7528\u7684\u6559\u6750\u4e3a\u300aProbabilistic Robotics\u300b<\/a>
\n<\/p>\n

RECURSIVE STATE ESTIMATION<\/h2>\n

Introduction<\/h3>\n

State estimation seeks to recover state variables from the data. Probabilistic state estimation algorithms compute belief distributions over possible world states.<\/p><\/blockquote>\n

\u5728\u6982\u7387\u673a\u5668\u4eba\u5b66\u4e2d\uff0c\u6211\u4eec\u4f9d\u9760\u4e00\u4e9b\u7b80\u5355\u7684\u6982\u7387\u8bba\u89c4\u5219\u6765\u8fdb\u884c\u673a\u5668\u4eba\u7684\u5efa\u6a21\uff0c\u5c24\u5176\u662fBayes\u6cd5\u5219<\/p>\n

\u8fd9\u91cc\u7279\u522b\u63d0\u4e00\u4e0bBayes\u7684\u76f8\u5173\u516c\u5f0f\u662f<\/p>\n

$$!p(a|b)=\\frac{p(b|a)\\cdot p(a)}{p(b)}$$
\n\u5176\u4e2d
\n$$!p(b)=p(b|a_1)p(a_1)da_1$$<\/p>\n

\u53e6\u5916\u662f\u6211\u4eec\u5b9a\u4e49$$x_t$$\u8868\u793a\u7cfb\u7edf\u7684\u5b8c\u5168\u72b6\u6001\uff0c$$u_t$$\u4e3a\u4f20\u611f\u5668\u52a8\u4f5c\uff0c$$z_t$$ \u4e3a\u5f53\u4e0b\u7684\u4f20\u611f\u5668\u6240\u6d4b\u5b9a\u7684\u6570\u503c\uff0c\u4e3a\u65b9\u4fbf\u4e00\u822c\u53d6\u65f6\u95f4\u4e3a\u79bb\u6563\u91cf\u3002<\/p>\n

\u53e6\u5916\uff0c$$x_{1:t}$$\u8868\u793a\u8fc7\u53bb1-t\u65f6\u95f4\u5185x\u7684\u96c6\u5408<\/p>\n

Bayes Filter<\/h3>\n

\u6240\u8c13 Bayes filter\u662f\u6982\u7387\u8bba\u4e00\u4e2a\u4e0d\u9519\u7684\u5e94\u7528\uff0c\u5927\u89c4\u6a21\u4f7f\u7528\u4e86Bayes\u6cd5\u5219<\/p>\n

Belief Distributions<\/h4>\n

Bayes Filter \u7684\u6838\u5fc3\u5728\u4e8e\u5bf9\u4fe1\u5ff5\u5206\u5e03\u7684\u64cd\u4f5c\uff0c\u6240\u8c13\u4fe1\u5ff5\u5206\u5e03\uff0c\u662f\u5728\u5df2\u77e5\u4f20\u611f\u5668\u6570\u636e\uff0c\u52a8\u4f5c\u5668\u52a8\u4f5c\u7684\u60c5\u51b5\u4e0b\u5bf9\u72b6\u6001$$x_t$$\u7684\u4e00\u4e2a\u4f30\u8ba1
\n\u5148\u5b9a\u4e49<\/p>\n

$$!bel(x_t)=p(x|z_{1:t},u_{1:t})$$
\n$$!\\overline{bel(x_t)}=p(x|z_{1:t-1},u_{1:t})$$<\/p>\n

\u4e5f\u5c31\u662f\u8bf4\uff0c$$bel(x_t)$$\u662f\u7528\u4e8e\u8868\u793a\u5728\u7ed9\u5b9a\u7684\u6d4b\u91cf\u5e8f\u5217\u548c\u52a8\u4f5c\u5e8f\u5217\u7684\u57fa\u7840\u4e0ax\u7684\u5206\u5e03\uff0c$$\\overline{bel(x_t)}$$\u5219\u662f\u77e5\u9053t-1\u65f6\u523b\u7684\u4fe1\u606f\u540e\u5bf9\u4e8et\u65f6\u523b\u7684$$u_t$$\u7684\u4e00\u4e2a\u9884\u6d4b\u3002<\/p>\n

Bayes \u7684\u8fed\u4ee3\u89c4\u5219<\/h4>\n

\u5728\u8fd9\u91cc\uff0c\u6211\u4eec\u53ef\u4ee5\u8f7b\u677e\u8bc1\u660e$$bel(x_t)$$,$$\\overline{bel(x_t)}$$\u548c$$bel(x_{t-1})$$\u4e4b\u95f4\u7684\u5173\u7cfb<\/p>\n

$$!\\overline{bel(x_t)}=\\int p(x_t|u_t,x_{t-1}) bel(x_{t-1})dx$$
\n$$!bel(x_t)=\\frac{p(z_t|x_t)\\overline{bel(x_t)}}{p(z_i)}$$<\/p>\n

$$p(z_i)$$\u4e3a\u5f52\u4e00\u5316\u5e38\u6570\uff0c\u6211\u4eec\u5728\u8fd9\u91cc\u4ec5\u4ec5\u8ba8\u8bba\u4e0a\u9762\u4e8c\u5f0f\u7684\u7269\u7406\u5b66\u610f\u4e49<\/p>\n

\u5148\u8bf4$$!\\overline{bel(x_t)}=\\int p(x_t|u_t,x_{t-1}) bel(x_{t-1})dx$$<\/p>\n

\u5728\u8fd9\u4e2a\u5f0f\u5b50\u91cc\u5982\u679c\u6211\u4eec\u5c06$$u_1:t-1$$ $$z_{1:t-1}$$\u5f53\u4f5c\u7ed9\u5b9a\u6761\u4ef6\uff0c\u90a3\u4e48bel(x_{t-1})\u5b9e\u9645\u4e0a\u610f\u5473\u7740\u4e0a\u4e00\u6b65\u5df2\u77e5\u7684\u5bf9\u72b6\u6001x\u7684\u4f30\u8ba1\u5206\u5e03\uff0c\u800c$$p(x_t|u_t,x_{t-1})$$\u6b63\u662f\u901a\u8fc7x\u7684\u4f30\u8ba1\u5206\u5e03\u548c\u4e0b\u4e00\u6b65\u5bf9\u5e94\u7684\u52a8\u4f5c\u6765\u8ba8\u8bba\u672a\u6765\u7684\u72b6\u6001\u5206\u5e03\uff0c\u5219\u5bb9\u6613\u7406\u89e3\u8fd9\u4e00\u5f0f\u5b50\u7684\u610f\u4e49\u662f\u6839\u636e\u5df2\u77e5\u7684\u6982\u7387\u5206\u5e03\u548c\u52a8\u4f5c$$u_t$$\u5bf9\u4e8e\u72b6\u6001\u6982\u7387\u5206\u5e03\u7684\u5f71\u54cd\u6765\u9884\u6d4b\u4e0b\u4e00\u6b65\u7684\u6982\u7387\u5206\u5e03\u3002<\/p>\n

\u53e6\u4e00\u4e2a\u5f0f\u5b50$$!bel(x_t)=\\eta p(z_t|x_t)\\overline{bel(x_t)}$$\u7684\u610f\u601d\u5219\u4e0d\u90a3\u4e48\u660e\u786e\uff0c\u5728\u8fd9\u91cc\u6211\u4eec\u7ed9\u51fa\u4e86\u5bf9\u4e8e$$bel(x_t)$$\u7684\u4e00\u4e2a\u9884\u6d4b$$\\overline{bel(x_t)}$$ \uff0c\u4f46\u662f\u8fd9\u91cc\u7684\u9884\u6d4b\u5e76\u4e0d\u4f9d\u8d56\u4e8e$$z_t$$\u7684\u51c6\u786e\u6d4b\u91cf\uff0c\u90a3\u4e48\u6211\u4eec\u53ef\u4ee5\u5229\u7528bayes\u6cd5\u5219\u6765\u66f4\u65b0\u4ed6\uff0c\u5229\u7528
\n$$x_t$$,$$z_t$$\u4e4b\u95f4\u7684\u5173\u7cfb\uff0c\u901a\u8fc7\u4e58\u56e0\u5b50$$p(z_t|x_t)$$\u548c\u5f52\u4e00\u5316\u540e\uff0c\u6211\u4eec\u5f97\u5230\u4e86\u5bf9\u4e8ex\u4f30\u8ba1\u7684\u5b8c\u6574\u503c\u3002<\/p>\n

\u4e00\u4e2a\u7b80\u5355\u7684\u6817\u5b50<\/h4>\n

\u4e0a\u9762\u8fd9\u4e48\u591a\u7ed5\u53e3\u7684\u516c\u5f0f\u5e76\u4e0d\u8db3\u4ee5\u76f4\u89c2\u7406\u89e3\uff0c\u8fd9\u91cc\u6211\u5f15\u7528\u4e66\u4e2d\u7684\u4e00\u4e2a\u6817\u5b50<\/p>\n

\"\u5c4f\u5e55\u5feb\u7167<\/a><\/p>\n

\u4e00\u4e2a\u840c\u840c\u54d2\u673a\u5668\u4eba\u8d70\u5230\u4e00\u6247\u95e8\u524d\uff0c\u4ed6\u9700\u8981\u786e\u5b9a\u95e8\u662f\u5426\u662f\u5f00\u7740\u7684\uff0c\u5e76\u4e14\u5927\u559d\u4e00\u58f0\u829d\u9ebb\u5f00\u95e8\u6765\u6253\u5f00\u8fd9\u6247\u95e8\u3002<\/p>\n

\u8fd9\u91cc\u4e3a\u4e86\u4e66\u5199\u65b9\u4fbf\uff0c\u6211\u4eec\u8fd9\u91cc\u5927\u5199\u5b57\u6bcd\u8868\u793a\u72b6\u6001\u53d8\u91cf\uff0c\u5c0f\u5199\u5b57\u6bcd\u8868\u793a\u72b6\u6001\u7684\u672c\u5f81\u503c\uff0c\u6bd4\u5982 $$x_0$$\u8868\u793a\u95e8\u6253\u5f00\uff0c $$ !x_0$$ \u8868\u793a\u95e8\u5173\u95ed
\n\u9996\u5148\uff0c\u6211\u4eec\u5148\u8ba4\u4e3a\u95e8\u5728\u673a\u5668\u4eba\u5230\u6765\u4e4b\u524d\u7684\u72b6\u6001\u662f\u968f\u673a\u7684\uff0c\u8fd9\u662f\u5bf9\u4e8e\u72b6\u6001x\u7684\u5148\u9a8c\u6982\u7387\u4e00\u4e2a\u4f30\u8ba1\uff0c\u4e5f\u5c31\u662f<\/p>\n

$$!bel(x_0)=0.5$$
\n$$!bel(!x_0)=0.5$$<\/p>\n

\u6211\u4eec\u5047\u8bbe\u840c\u840c\u54d2\u673a\u5668\u4eba\u7684\u4f20\u611f\u5668\u5de5\u4f5c\u5e76\u4e0d\u7a33\u5b9a(\u8fd9\u91cc$$z_t$$,$$ !z_t$$\u5206\u522b\u8868\u793a\u4f20\u611f\u5668\u89c2\u6d4b\u5230\u6253\u5f00\u6216\u8005\u5173\u95ed)\uff0c\u6bd4\u5982<\/p>\n

$$!p(x_t|x_t)=0.6$$
\n$$!p(!z_t|x_t)=0.4$$
\n$$!p(z_t|!x_t)=0.2$$
\n$$!p(!z_t|!x_t)=0.8$$<\/p>\n

\u53e6\u5916\u662f\u5bf9\u4e8e\u6253\u5f00\u4e00\u6247\u95e8\uff0c\u673a\u5668\u4eba\u53ea\u670980%\u7684\u6210\u529f\u6982\u7387\uff0c\u4f46\u662f\u673a\u5668\u4eba\u5e76\u4e0d\u4f1a\u9020\u6210\u95e8\u7684\u610f\u5916\u5173\u95ed<\/p>\n

\u5373
\n$$!p(x_t|!x_{t-1},U_t)=0.8$$
\n$$!p(!x_t|!x_{t-1},U_t)=0.2$$<\/p>\n

$$!p(x_t|x_{t-1},U_t)=1$$
\n$$!p(!x_t|x_{t-1},U_t)=0$$<\/p>\n

\u90a3\u4e48\u6211\u4eec\u5f00\u59cb\u8fdb\u884c\u5206\u6790\u7cfb\u7edf\u7684\u53d8\u5316<\/p>\n

\u9996\u5148\u5199\u51fa\u9884\u6d4b\u8fed\u4ee3\u7684\u8868\u8fbe\u5f0f<\/p>\n

$$!\\overline{bel(X_t)}=\\int p(x_t|U_t,x_{t-1}) bel(x_{t-1})dx$$<\/p>\n

\u5728\u8fd9\u91cc<\/p>\n

$$!\\overline{bel(X_t)}=\\sum_{x_{t-1}} p(x_t|U_1,x_{t-1}) bel(x_{t-1})$$
\n$$!\\overline{bel{X_t}}=p(X_t|U_t,!x_{t-1}) bel(!x_{t-1})+p(X_{t}|U_t,x_{t-1}) bel(x_{t-1})$$<\/p>\n

\u5047\u8bbe$$U_1=!u$$\uff0c\u90a3\u4e48\u6709
\n$$!\\overline{bel{x_1}}=bel(!x_1)=0.5$$<\/p>\n

\u6b64\u65f6\uff0c\u5047\u8bbe\u6211\u4eec\u7684\u673a\u5668\u4eba\u89c2\u6d4b\u5230\u95e8\u662f\u6253\u5f00\u7684\uff0c
\n$$!bel(X_t)=\\eta \\cdot p(Z_t|X_t) \\overline{bel(X_t)} = p(z_t|X_t)\\overline{bel(x_t)}$$<\/p>\n

\u90a3\u4e48
\n$$!bel(x_1)=\\eta 0.6\\cdot 0.5=0.3\\eta$$
\n$$!bel(!x_1)=\\eta 0.2\\cdot 0.5=0.1\\eta$$<\/p>\n

\u5f52\u4e00\u5316\u540e<\/p>\n

$$!bel(x_1)=0.75; bel(!x_1)=0.25 $$<\/p>\n

\u597d\u8fd9\u4e2a\u65f6\u5019\u6211\u4eec\u505a\u4e00\u4e2apush\u64cd\u4f5c<\/p>\n

$$!overline(x_2)=0.751+0.25<\/em>0.8=0.95$$
\n$$!overline(!x_2)=00.75+0.25<\/em>0.2=0.05$$<\/p>\n

\u4e5f\u5c31\u662f\u8bf4\uff0c\u6b64\u65f6\u6839\u636e\u9884\u6d4b\u95e8\u5904\u4e8e\u6253\u5f00\u7684\u6982\u7387\u4e3a0.95\uff0c<\/p>\n

\u8fd9\u4e2a\u65f6\u5019\u6211\u4eec\u53c8\u89c2\u6d4b\u5230\u4e86\u95e8\u6253\u5f00\uff0c\u90a3\u4e48$$bel(x2)=0.983$$ $$bel(!x2)=0.017$$<\/p>\n

\u4e5f\u5c31\u662f\u6211\u4eec\u89c2\u6d4b\u5230\u95e8\u662f\u5f00\u7684\uff0c\u518d\u53bb\u5f00\u95e8\uff0c\u4ecd\u7136\u67090.017\u7684\u6982\u7387\u4f7f\u5f97\u95e8\u662f\u5173\u7684\uff0c\u5f53\u7136\u4e86\uff0c\u6211\u4eec\u53ef\u4ee5\u91cd\u590d\u8fd9\u4e00\u52a8\u4f5c\u8fdb\u884c\u8fed\u4ee3\u3002<\/p>\n

\u603b\u7ed3<\/h4>\n

\u5173\u4e8eBayes Filter\u7684\u57fa\u672c\u5185\u5bb9\u5c31\u662f\u8fd9\u4e48\u591a\uff0c\u53ef\u4ee5\u770b\u5230\u8fd9\u662f\u4e00\u79cd\u4f7f\u7528\u6982\u7387\u6765\u5206\u6790\u6bcf\u4e00\u4e2a\u8fc7\u7a0b\u8fc1\u79fb\u7684\uff0c\u540e\u9762\u8fd8\u6709\u4e00\u4e9b\u5173\u4e8e\u9a6c\u5c14\u53ef\u592b\u8fc7\u7a0b\u7684\u5206\u6790\uff0c\u5c31\u4e0d\u8bb2\u4e86\uff0c\u8fd9\u91cc\u6211\u4eec\u65e0\u59a8\u6bd4\u8f83\u4e00\u4e0bBayes Filter\u548c\u91cf\u5b50\u529b\u5b66\u7684\u5f02\u540c\u3002<\/p>\n

\u9996\u5148\u662f\uff0c\u5bf9\u4e8e\u7269\u4f53\uff0c\u6211\u4eec\u4ec5\u4ec5\u77e5\u9053\u5176\u5904\u4e8e\u4e00\u4e9b\u672c\u5f81\u6001\u7684\u6982\u7387\u53e0\u52a0\uff0c\u6211\u4eec\u65e0\u6cd5\u51c6\u786e\u7684\u8bf4\uff0c\u5230\u5e95\u7269\u4f53\u5904\u4e8e\u54ea\u4e2a\u6001\uff0c\u4f46\u662f\u4e0d\u540c\u7684\u662f\uff0c\u4e00\u6b21\u91cf\u5b50\u529b\u5b66\u89c2\u6d4b\u53ef\u4ee5\u4f7f\u7269\u4f53\u574d\u584c\u5230\u89c2\u6d4b\u77e9\u9635\u7684\u4e00\u4e2a\u672c\u5f81\u503c\uff0c\u800c\u4e14\u5c31\u8fd9\u4e48\u4e0d\u53d8\u4e0b\u53bb\uff0c\u4f46\u662f\u5bf9\u4e8eBayes Filter\uff0c\u5c3d\u7ba1\u89c2\u6d4b\u574d\u584c\u4e86\uff0c\u6211\u4eec\u8fd8\u662f\u4e0d\u80fd\u8bf4\uff0c\u8fd9\u4e2a\u7c92\u5b50\u5c31\u5904\u4e8e\u4e86\u8fd9\u4e2a\u6001\uff0c\u4ec5\u4ec5\u662f\u5bf9\u4e8e\u5148\u9a8c\u6982\u7387\u7684\u4e00\u6b21\u8fed\u4ee3\u52a0\u5f3a\u3002<\/p>\n

\u53e6\u5916\u6211\u60f3\u8ba8\u8bba\u662f\u5426\u6709\u8fd9\u4e48\u4e00\u4e2a\u53ef\u80fd\u6027\uff0c\u5bf9\u4e8e\u4ec0\u4e48\u60c5\u51b5\u4e0b\uff0c\u7ecf\u8fc7\u65e0\u6570\u6b21\u89c2\u6d4b\uff0c\u7269\u4f53\u7684\u89c2\u6d4b\u503c\u4f1a\u8d8b\u5411\u4e8e\u60010\u3002<\/p>\n

\u5173\u952e\u5e94\u5f53\u5728\u4e8e$$p(z_t|x_t)$$\u4e5f\u5c31\u662f\u4f20\u611f\u5668\u7684\u7075\u654f\u5ea6\u3002<\/p>\n","protected":false},"excerpt":{"rendered":"

\u5047\u671f\u5b66\u4e60Probabilistic Robotics\u7684\u4e00\u4e9b\u7b14\u8bb0 \u4f7f\u7528\u7684\u6559\u6750\u4e3a\u300aProbabilistic Ro … <\/p>\n

\u7ee7\u7eed\u9605\u8bfb\u201cProbabilistic Robotics\u5b66\u4e60\u7b14\u8bb0: Bayes Filter \u57fa\u672c\u6982\u5ff5\u201d<\/span><\/a><\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":[],"categories":[11],"tags":[],"_links":{"self":[{"href":"http:\/\/blog.xuhao1.me\/index.php?rest_route=\/wp\/v2\/posts\/590"}],"collection":[{"href":"http:\/\/blog.xuhao1.me\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"http:\/\/blog.xuhao1.me\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"http:\/\/blog.xuhao1.me\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"http:\/\/blog.xuhao1.me\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=590"}],"version-history":[{"count":29,"href":"http:\/\/blog.xuhao1.me\/index.php?rest_route=\/wp\/v2\/posts\/590\/revisions"}],"predecessor-version":[{"id":620,"href":"http:\/\/blog.xuhao1.me\/index.php?rest_route=\/wp\/v2\/posts\/590\/revisions\/620"}],"wp:attachment":[{"href":"http:\/\/blog.xuhao1.me\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=590"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"http:\/\/blog.xuhao1.me\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=590"},{"taxonomy":"post_tag","embeddable":true,"href":"http:\/\/blog.xuhao1.me\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=590"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}