Revision 1821
Individual Wheel Speeds
| sensorModel.m | ||
|---|---|---|
| 19 | 19 |
% TODO: model encoder error? |
| 20 | 20 |
% Use the lagged values for position |
| 21 | 21 |
|
| 22 |
noise = noise + encoderNoise .* (encoderNoiseVar * abs(randn(2, n))); |
|
| 23 | 22 |
|
| 23 |
noise = noise + (encoderNoise .* (encoderNoiseVar * abs(randn(2, n))) ./ wheels); |
|
| 24 | 24 |
|
| 25 |
|
|
| 25 | 26 |
R = 3.5; |
| 26 | 27 |
L = 12.75; |
| 27 | 28 |
|
| 28 | 29 |
transform = [ R/2 R/2; -R/L R/L]; |
| 29 | 30 |
|
| 31 |
|
|
| 32 |
|
|
| 30 | 33 |
for i=2:n, |
| 31 | 34 |
|
| 32 |
wheels = wheels + noise(:, i); |
|
| 33 |
q = transform * wheels; |
|
| 35 |
|
|
| 36 |
wheels(:, i) = wheels(:, i) + noise(:, i) ; |
|
| 37 |
q = transform * wheels(:, i); |
|
| 34 | 38 |
v(i) = q(1); |
| 35 | 39 |
omega(i) = q(2); |
| 40 |
% disp(wheels) |
|
| 36 | 41 |
|
| 37 | 42 |
end |
| 38 | 43 |
|
| 44 |
disp(wheels) |
|
| 45 |
|
|
| 39 | 46 |
xSensor = xoldSensed; |
| 40 | 47 |
ySensor = yoldSensed; |
| 41 | 48 |
thetaSensor = thetaoldSensed; |
Also available in: Unified diff