练习 1
在机器人定位的课程中,Sebastian Thrun 已经带领你了解了一维机器人传感和运动的 Python 代码。最后你得到了一个 sense() 函数,它基于传感器的测量数据来更新概率。你也得到了一个 move() 函数,它根据机器人在网格中的运动更新概率。
现在请将下面的 Python 代码翻译成 C++ 代码:
p = [0.2, 0.2, 0.2, 0.2, 0.2] world = ['green', 'red', 'red', 'green', 'green'] measurements = ['red', 'green'] motions = [1,1] pHit = 0.6 pMiss = 0.2 pExact = 0.8 pOvershoot = 0.1 pUndershoot = 0.1 def sense(p, Z): q=[] for i in range(len(p)): hit = (Z == world[i]) q.append(p[i] * (hit * pHit + (1-hit) * pMiss)) s = sum(q) for i in range(len(q)): q[i] = q[i] / s return q def move(p, U): q = [] for i in range(len(p)): s = pExact * p[(i-U) % len(p)] s = s + pOvershoot * p[(i-U-1) % len(p)] s = s + pUndershoot * p[(i-U+1) % len(p)] q.append(s) return q for k in range(len(measurements)): p = sense(p, measurements[k]) p = move(p, motions[k]) print p
我改写的C++版本:
#include <iostream>
#include <vector>
using namespace std;
vector<float> p(5,0.2);
vector<string> world = {"green", "red", "red", "green", "green"};
vector<string> measurements = {"red","green"};
vector<int> motions = {1,1};
float pHit,pMiss,pExact,pOvershoot,pUndershoot;
vector<float> sense(vector<float> p,string Z){
// for (int i=0;i<p.size();i++){
// cout<<p[i]<<endl;
// }
pHit = 0.6;
pMiss = 0.2;
vector<float> q;
int hit;
for (int i = 0; i < p.size();i++){
hit = (Z == world[i]);
// cout<<hit<<endl;
// cout<<p[i]* (hit * pHit + (1-hit) * pMiss)<<endl;
q.push_back(p[i]* (hit * pHit + (1-hit) * pMiss));
}
float s = 0;
for (int i = 0; i < q.size();i++){
s = s + q[i];
}
for (int i = 0; i < q.size();i++){
q[i] = q[i] / s;
}
return q;
}
vector<float> move(vector<float> p, int U){
vector<float> q;
float s;
pOvershoot = 0.1;
pUndershoot = 0.1;
pExact = 0.8;
for (int i=0;i<p.size();i++){
s = pExact * p[(i-U) % p.size()];
s = s + pOvershoot * p[(i-U-1) % p.size()];
s = s + pUndershoot * p[(i-U+1) % p.size()];
q.push_back(s);
}
return q;
}
int main()
{
// vector<float> p(5,0.2);
// vector<string> world = {"green", "red", "red", "green", "green"};
// vector<string> measurements = {"red","green"};
// vector<int> motions = {1,1};
// float pHit,pMiss,pExact,pOvershoot,pUndershoot;
for (int i=0;i<measurements.size();i++){
p = sense(p,measurements[i]);
p = move(p,motions[i]);
}
for (int i=0;i<p.size();i++){
cout<<p[i]<<endl;
}
// cout << p << endl;
// for (int i;i<world.size();i++){
// cout<<world[i]<<endl;
// }
// cout << "Hello world!" << endl;
return 0;
}
显示结果: