Revision 1818
some bugfixes, stuff still isn't working though
sensorModel.m | ||
---|---|---|
1 | 1 |
%TODO: pass in dt for lag model |
2 |
function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensor, yoldSensor, thetaoldSensor] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor)
|
|
2 |
function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensed, yoldSensed, thetaoldSensed] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensed, yoldSensed, thetaoldSensed, dt)
|
|
3 | 3 |
|
4 | 4 |
phiNoiseVar = 0.1; |
5 |
encoderNoiseVar = 0.1; |
|
5 |
encoderNoiseVar = 0; %0.1;
|
|
6 | 6 |
noiseMean = 0; |
7 | 7 |
|
8 | 8 |
%TODO: value in time instead of number of calls |
... | ... | |
18 | 18 |
|
19 | 19 |
% TODO: model encoder error? |
20 | 20 |
% Use the lagged values for position |
21 |
noise = encoderNoise * encoderNoiseVar * abs(randn(2, n)); |
|
22 | 21 |
|
22 |
noise = encoderNoise .* (encoderNoiseVar * abs(randn(2, n))); |
|
23 |
|
|
23 | 24 |
R = 3.5; |
24 | 25 |
L = 12.75; |
25 | 26 |
|
26 | 27 |
transform = [ R/2 R/2; -R/L R/L]; |
27 | 28 |
|
28 |
for i=2:numRobots,
|
|
29 |
for i=2:n, |
|
29 | 30 |
|
30 | 31 |
wheels = wheels + noise(:, i); |
31 | 32 |
q = transform * wheels; |
... | ... | |
34 | 35 |
|
35 | 36 |
end |
36 | 37 |
|
37 |
x = zeros(numRobots,1); |
|
38 |
y = zeros(numRobots,1); |
|
39 |
theta = zeros(numRobots,1); |
|
40 |
|
|
41 | 38 |
xSensor = xoldSensed; |
42 | 39 |
ySensor = yoldSensed; |
43 | 40 |
thetaSensor = thetaoldSensed; |
44 | 41 |
|
45 |
xoldSensed = xoldSensed + cos(thetaoldSensed)*v(i)*dt;
|
|
46 |
yoldSensed = yoldSensed + sin(thetaoldSensed)*v(i)*dt;
|
|
47 |
thetaoldSensed = thetaoldSensed + omega*dt;
|
|
42 |
xoldSensed = xoldSensed + cos(thetaoldSensed).*v*dt;
|
|
43 |
yoldSensed = yoldSensed + sin(thetaoldSensed).*v*dt;
|
|
44 |
thetaoldSensed = thetaoldSensed + omega*dt; |
|
48 | 45 |
|
49 | 46 |
|
50 |
|
|
51 |
|
|
52 |
|
|
53 |
|
|
54 | 47 |
% phiSensor is the value from the BOM sensor |
55 | 48 |
% round past phi to the nearest pi/8 |
56 | 49 |
noisePhi = state.phi(1,:)' + randn(1,size(phiTrue,1))'*phiNoiseVar + noiseMean; |
Also available in: Unified diff