Revision 1711
added encoder lag to sensor model
| sensorModel.m | ||
|---|---|---|
| 6 | 6 |
|
| 7 | 7 |
% state contains historical phi values |
| 8 | 8 |
if size(state,1) == 0 |
| 9 |
state = zeros(sensorLag,size(phiTrue,2)); |
|
| 9 |
state.phi = zeros(sensorLag,size(phiTrue,1)); |
|
| 10 |
state.x = zeros(sensorLag,size(phiTrue,1)); |
|
| 11 |
state.y = zeros(sensorLag,size(phiTrue,1)); |
|
| 12 |
state.theta = zeros(sensorLag,size(phiTrue,1)); |
|
| 10 | 13 |
end |
| 11 | 14 |
|
| 12 | 15 |
% TODO: model encoder error? |
| 13 |
xSensor = xTrue; |
|
| 14 |
ySensor = yTrue; |
|
| 15 |
thetaSensor = thetaTrue; |
|
| 16 |
% Use the lagged values for position |
|
| 17 |
xSensor = state.x(1,:); |
|
| 18 |
ySensor = state.y(1,:); |
|
| 19 |
thetaSensor = state.theta(1,:); |
|
| 16 | 20 |
|
| 17 | 21 |
% phiSensor is the value from the BOM sensor |
| 18 |
|
|
| 19 | 22 |
% round past phi to the nearest pi/8 |
| 20 |
phiSensor = round(state(1,:)*8/pi)*pi/8; |
|
| 23 |
phiSensor = round(state.phi(1,:)*8/pi)*pi/8;
|
|
| 21 | 24 |
|
| 22 | 25 |
% update the state |
| 23 |
state(1:end-1,:) = state(2:end,:); |
|
| 24 |
state(end,:) = phiTrue'; |
|
| 26 |
state.x(1:end-1,:) = state.x(2:end,:); |
|
| 27 |
state.y(1:end-1,:) = state.y(2:end,:); |
|
| 28 |
state.theta(1:end-1,:) = state.theta(2:end,:); |
|
| 29 |
state.x(end,:) = xTrue'; |
|
| 30 |
state.y(end,:) = yTrue'; |
|
| 31 |
state.theta(end,:) = thetaTrue'; |
|
| 32 |
state.phi(end,:) = phiTrue'; |
|
| 25 | 33 |
|
| 26 | 34 |
end |
Also available in: Unified diff