root / branches / 16299_s10 / matlab / sensorModel.m @ 1821
History | View | Annotate | Download (1.94 KB)
1 |
%TODO: pass in dt for lag model |
---|---|
2 |
function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensed, yoldSensed, thetaoldSensed, noise] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensed, yoldSensed, thetaoldSensed, dt, noise) |
3 |
|
4 |
phiNoiseVar = 0.1; |
5 |
encoderNoiseVar = 0.05; |
6 |
noiseMean = 0; |
7 |
|
8 |
%TODO: value in time instead of number of calls |
9 |
sensorLag = 2; |
10 |
|
11 |
% state contains historical phi values |
12 |
if size(state,1) == 0 |
13 |
state.phi = zeros(sensorLag,size(phiTrue,1)); |
14 |
state.x = zeros(sensorLag,size(phiTrue,1)); |
15 |
state.y = zeros(sensorLag,size(phiTrue,1)); |
16 |
state.theta = zeros(sensorLag,size(phiTrue,1)); |
17 |
end |
18 |
|
19 |
% TODO: model encoder error? |
20 |
% Use the lagged values for position |
21 |
|
22 |
|
23 |
noise = noise + (encoderNoise .* (encoderNoiseVar * abs(randn(2, n))) ./ wheels); |
24 |
|
25 |
|
26 |
R = 3.5; |
27 |
L = 12.75; |
28 |
|
29 |
transform = [ R/2 R/2; -R/L R/L]; |
30 |
|
31 |
|
32 |
|
33 |
for i=2:n, |
34 |
|
35 |
|
36 |
wheels(:, i) = wheels(:, i) + noise(:, i) ; |
37 |
q = transform * wheels(:, i); |
38 |
v(i) = q(1); |
39 |
omega(i) = q(2); |
40 |
% disp(wheels) |
41 |
|
42 |
end |
43 |
|
44 |
disp(wheels) |
45 |
|
46 |
xSensor = xoldSensed; |
47 |
ySensor = yoldSensed; |
48 |
thetaSensor = thetaoldSensed; |
49 |
|
50 |
% xoldSensed = xoldSensed + cos(thetaoldSensed).*v*dt; |
51 |
% yoldSensed = yoldSensed + sin(thetaoldSensed).*v*dt; |
52 |
% thetaoldSensed = thetaoldSensed + omega*dt; |
53 |
|
54 |
xoldSensed = state.x(1,:) + cos(state.theta(1,:)).*v*dt; |
55 |
yoldSensed = state.y(1,:) + sin(state.theta(1,:)).*v*dt; |
56 |
thetaoldSensed = state.theta(1,:) + omega*dt; |
57 |
|
58 |
|
59 |
|
60 |
% phiSensor is the value from the BOM sensor |
61 |
% round past phi to the nearest pi/8 |
62 |
noisePhi = state.phi(1,:)' + randn(1,size(phiTrue,1))'*phiNoiseVar + noiseMean; |
63 |
phiSensor = round(noisePhi*8/pi)*pi/8; |
64 |
%phiSensor = round(state.phi(1,:)'*8/pi)*pi/8; |
65 |
|
66 |
% update the state |
67 |
state.x(1:end-1,:) = state.x(2:end,:); |
68 |
state.y(1:end-1,:) = state.y(2:end,:); |
69 |
state.theta(1:end-1,:) = state.theta(2:end,:); |
70 |
state.phi(1:end-1,:) = state.phi(2:end,:); |
71 |
state.x(end,:) = xTrue'; |
72 |
state.y(end,:) = yTrue'; |
73 |
state.theta(end,:) = thetaTrue'; |
74 |
state.phi(end,:) = phiTrue'; |
75 |
|
76 |
end |