Home
Hybrid Indoor Geolocation for Robotic Applications
Contents
1. Create the Move Control controlLabel new JLabel Click Here To Control JLabel CENTER controlLabel setOpaque true controlLabel setBackground Color RED controlLabel setFocusable true controlLabel addFocusListener this controlLabel addMouseListener this CameraPanel cp new CameraPanel cp setSize 400 400 add add add add cp lblVelocity velocitySlider controlLabel anana Override public void focusGained FocusEvent e moveController updateControlLabel true Override public void focusLost FocusEvent e moveController updateControlLabel false Override public void mouseClicked MouseEvent arg0 TODO Auto generated method stub moveController updateControlLabel true Override public void mouseEntered MouseEvent arg0 TODO Auto generated method stub Override public void mouseExited MouseEvent arg0 TODO Auto generated method stub 113 Override public void mousePressed MouseEvent arg0 TODO Auto generated method stub Override public void mouseReleased MouseEvent arg0 TODO Auto generated method stub Appendix 0 3 5 LoggerPanel java package views import javax swing import java awt event import java awt import models Robot import controllers LoggerController public class LoggerPanel extends JPanel private Robot ro
2. string msg imuSensor Matlab Execute G erInit MessageBox Show msg private void btnCloseCamera_Click object sender EventArgs e string msg imuSensor Matlab Execute run C Program Files MATLAB R2006b toolbox ERToolkits erClose nu string msg GetWIFISignalStrength Console WriteLine msg GetAvailableNetworks MessageBox Show msg private void btnUpdateImage Click object sender EventArgs e imuSensor Matlab Execute addpath C Program Files MATLAB R2006b toolbox ERToolkits 7 string msg imuSensor Matlab Execute img erGetImage imwrite img c current jpg Console WriteLine msg private void btnGetWiFi Click object sender EventArgs e f n string name int samples 0 try f name txtDBName Text samples int Parse txtNumSamples Text catch Exception ex MessageBox Show Invalid Name or Number of Samples chosen return wifiDB Name name wifiDB SamplesToRead samples wifiDB SamplesRead 0 192 private void btnClearDB Click object sender Ei lt ntArgs e wifiDB Clear private void btnSaveWiFiDB Click object sender EventArgs e wifiDB save true 193 BIBLIOGRAPHY 10 11 Miller L E P F Wilson N P Bryner M H Francis J R Guerrieri D W Stroup L Klein Berndt
3. return new ER1Position 1 1 if posStr Contains r n posStr posStr Split n 0 Trim string pos posStr Split double x Double Parse pos 1 double y Double Parse pos 2 double angle Double Parse pos 3 return new ERIPosition x y x y coordinate in centimeters Simulation lic static ER1Position simPositions null lic static int numPositions 100 number of simulated positons lic static int Tmeasure 1000 Time in milliseconds between s lic static int index 0 Uses to keep track of which use for next simulated postion update lic static void genSimPositions Check if Simulated Positions Already Generated if simPositions null amp amp simPositions Length gt 0 return Generate new array of simulated ER1 Positions simPositions new ER1Position numPositions DateTime timestamp DateTime Now Timestamp of first Generate ER1 Positions in progressing in time for int i 0 i lt numPositions itt simPositions i new ER1Position i i timestamp timestamp timestamp AddMilliseconds Tmeasure public static ER1Position nextSimulatedPosition genSimPositions Generate Simulation Poistions 130 if index gt numPositions index 0 Reset simulation when end is reached return simPositions index Return the next ER1Position from simulated postions generated Appendix P 1 4
4. Store this recevied sensor data along with the rest of the past data ReceivedData Add newData if new data was recieved and the callback is defined pass new data to callback if newData Equals SensorData NoSensorData amp amp this SensorUpdateReceived null SensorUpdateReceived newData else Otherwise there was NoSensorData or no callback was defined Console WriteLine IMUController data newData SensorDataReceived SensorUpdateReceived n override public void doUpdate object sender System Timers ElapsedEventArgs e Called every time interval double dt total 0 0 0 0 double dh total int length this data Count lastCount if length lt 1 Console WriteLine IMU no IMU values over this TimeInterval ms to integrate into EKF return IMUData data new IMUData length for int i lastCount 1 i lt this data Count itt IMUData curr i gt 1 IMUData this data 1 IMUData this CurrentData IMUData last i gt 0 IMUData this data i 1 null double dt this getTimeDiff curr last double dh curr AngularVelocity dt dt total dt dh total dh if Math Abs dh total gt Math PI 2 f Console WriteLine IMU Large change in heading over 180 deg dh total so forcing to ZERO dh total 0 150 if double IsInfinity dh total double IsNaN dh total
5. matlab PutFullMatrix x base State dummy matlab PutFullMatrix P base Covariance dummy2 matlab PutCharArray state type base EKFState ReceivedData i Type matlab PutCharArray state time base EKFState ReceivedData i Timestamp ToString matlab Execute xSaved i 1 x matlab Execute pSaved i 1 P matlab Execute state info i 1 state type state time matlab Execute save EKFStateLog mat xSaved matlab Execute save EKFCovLog mat pSaved matlab Execute save EKFInfo mat state info 167 catch System Runtime InteropServices COMException comEx Catched MATLAB Exception Display Error string message Matlab Command scriptName An Kili message Matlab Response response An message Exception comEx Message Mn MessageBox Show message public static EKFState CurrentState get return currentState set currentState value Update the current position using Robot GyrX Angular Velocity using IMU c s Velocity using ER1 and public static void Time Update double deltaPosition double deltaHeading lock currentState Array State Covariance Displacement dummy dummy2 dummy3 Get the current state and covariance x P and give to MATLAB State
6. Returns whether the sensor is polling or not active public bool isUpdating return updating Pad Summary Pi Removes all elements from the ReceivedData public void Clear if data null data new ArrayList If ArrayList undefined instantiate it data Clear Empty ArrayList of Sensor Data The rate at which new values are read from sensor public int TimeInterval get return interval set interval value Array of all received Sensor Data 139 public ArrayList ReceivedData get return data set data value Get the most recently received value from data public SensorData getCurrentData Handle no received data if data null data Count 0 return SensorData NoSensorData Return the last Sensor Data in array return SensorData data data Count 1 The most recent received Sensor Data public SensorData CurrentData get return getCurrentData Get the last received Sensor Data set data Add value Add the latest received Sensor Data to the ReceievedData array public abstract bool handlesRequest string request public static bool handlesRequest string request for int i 0 i lt validRequests Length i if request StartsWith validRequests i return true return false aed public double getTimeDiff SensorData current SensorData last
7. eite ee tenetis deed eenddes 105 Appendix O 3 Robot Remote Control Client ER1Navigator Views eene 107 Appendix O 3 1 CameraPanel java iii recibe tesi terree adt GA Nd 107 Appendix 0 3 2 CommarndPaneljaya io eot a Jana 109 Appendix 0 3 3 ConnecaionPanel java ect o taedio iata NAAN 110 Appendix 0 34 ControlPanel jaya erit ine de e pii esas 112 Appendix O 3 5 LoggerPanel Java aee eder eh an ter eed eg es Rocio dee ar due dU HUP 114 Appendix O 3 6 MapPanel java maa NANANG RANA esa o Ue Pese aeo MAKA 117 Appendix 0 3 7 Map View java dol ioc atte tierce etc dede ac eos seti uut 120 Appendix 0 3 8 IRODOEVIEW Ja deseos it ss cte ANAN Dap AA 121 Appendix P 1 Robot Server Models aaa ah iita AA dele d aieo deeds 123 Appendix PAs SensorDGaltd 65 naala paa attente reales canes a Laut ngaa 123 Appendix P 1 2 EKEStAale C8 est NG BKA GRANADA Mhal 124 Appendix P3 JER POSIMONCS asc Sun teat detiene te taies daa Baa aasa 128 Appendix P 14 IM Data OS aoi e o eri eret ett aasa 131 Appendix PALS WIP ata CSi eaaet rra aes Rate ddp ee de tonis apis ee 133 Appendix P T 67 SonarData es oonetecdnt o see o oit et e hei hoiati hha 136 Appendix P 2 Robot Server Controllers 5 iere cede itid tee et iaa uad 138 Appendix P 2 1 SensorController cs iuo iip un dece ot bU eruta inus 138 Appendix P 2 2 BRI ControlleB 6 Susi ia ia PN tech ad dale LNAG 141 Appendix P 2 3 IMUControllet 68 24 5 aee hir B
8. catch System Runtime InteropServices COMException comEx Catched MATLAB Exception Display Error string message Error exporting IMU Data to Matlab n message Matlab Command scriptName An message Exception comEx Message n MessageBox Show message public MLApp MLAppClass Matlab get return matlab set matlab value public string ScriptName get return scriptName set scriptName value static public bool handlesRequest string request for int i 0 i lt validRequests Length i if request StartsWith validRequests il return true return false override public string ToString string response IMUController Matlab Script scriptName wie respons t base ToString return response Appendix P 2 4 WiFiController cs using System using System Collections Generic using System Ling using System Text using System Windows Forms 152 using ERlLocationListener Models namespace ERlLocationListener Controllers class WiFiController SensorController private static string validRequests getWiFiValues Requests to be handles by WiFi Controller private MLApp MLAppClass matlab Create MATLAB object to run scripts private string scriptName Matlab Script to Execute to Read Values from WiFi private string scri
9. public static SensorUpdateReceievedDelegate SensorUpdateReceived 163 return didUpdate didUpdate value gei se Setup CustomAlgorithm to use SensorControllers and Sensor Data to track update position and angle public static void InitEKF Set up matlab interface try ReceivedData new ArrayList didUpdate null matlab new MLApp MLAppClass Instantiate Matlab Engine scriptName Time Update Name of Matlab Script to run to get IMU Values catch System Runtime InteropServices COMException comEx System Windows Forms MessageBox Show Message comEx Message nStackTrace comEx StackTrace Ask to Initialize Robot s Position to X Y angle in centimeters and degrees as well as velocity and angular velocity cm s and rad s and covariances DialogResult dr MessageBox Show Do you want to initialize the EKF State Initalize EKF MessageBoxButtons YesNo if dr Equals DialogResult Yes currentState new EKFState Initialize State else Array state new double 5 Array covariance new double 5 5 currentState new EKFState state covariance public static void Initialize State EKFState newState null lock currentState Initalize EKF state and covariance arrays Array state new double EKFState length Array dummy new double state Length
10. public void actionPerformed ActionEvent e start replay logControlPanel add btnReplay a Add Clear Log Button btnClearLog new JButton Clear Log btnClearLog addActionListener new ActionListener public void actionPerformed ActionEvent e clear log logControlPanel add btnClearLog Add Log Window txtLogArea new JTextArea scrollLogArea new JScrollPane txtLogArea Add Log Pane logTabPane new JTabbedPane logTabPane addTab Log Control logControlPanel logTabPane addTab ER1 Robot Log scrollLogArea add logTabPane 115 public void select file int result fcLogFile showSaveDialog this if result JFileChoos r CANCEL OPTION return txtLogFile setText fcLogFile getSelectedFile getName public void clear log logger clearLog public void start logging if logger isLogging logger startLogging btnStartLogging setText Stop Logging else logger stopLogging btnStartLogging setText Start Logging update Replay button label updateReplayButtonLabel public void updateStartButtonLabel if logger isLogging btnStartLogging setText Stop Logging else btnStartLogging setText Start Logging public void start replay f if logger isReplay f logger startReplay btnReplay setText Stop R
11. CommonDialog cm new CommonDialog using System De Device d WIAVIDEOLib WiaVideoClass wvc new WIAVIDEOLib WiaVideoClass wvc CreateVideoByDevNum void frameTimer Tick object sender EventArgs e f btnUpdatelmage Click sender e private void ERlPositionReceived Models SensorData ERlPosition Adds ER1Postion to ER1 Data Grid View SafeDataGridAddRow grdER1Position ERIPosition Check Sensor Updating was Stopped if ER1LPosition Equals Models SensorData SensorUpdatingStopped 181 Update UI SafeRenameButton btnStartUpdatingER1 Start Updating ER1 private void IMUUpdateReceived Models SensorData imuData Check Sensor Updating was Stopped if imuData Equals Models SensorData SensorUpdatingStopped SafeRenameButton btnStartUpdatingIMU Start Updating IMU Adds IMU Data to IMU Data Grid View SafeDataGridAddRow dgvIMUData imuData private void WiFiUpdateReceived Models SensorData wifiData FJ KF ex priv H priv Check Sensor Updating was Stopped if wifiData Equals Models SensorData SensorUpdatingStopped SafeRenameButton btnStartUpdatingWiFi Stop Updating WiFi Adds WiFi Data to WiFi Data Grid View SafeDataGridAddRow dgvWiFiData wifiData try EKFAlgorithmClass WiFi Measurement Update wifiData catch ArgumentEx
12. Timestamp return imuSensor CurrentData ToString Handle position request use ER1 if connected else if request Equals position OVERRIDE ER1 Positon cmd for if erlController connected Simulation mode when no connection Models ERlPosition erlPos Models ER1Position erlController CurrentData if Models SensorData NoSensorData Equals erlPos return Not Connected to ER1 Telnet API return OK erlPos XPos ToString 1d T erlPos YPos ToString ld erlPos Angle ToString S 1d return erlPos ToResponseString return erlController sendCommand request Models ERlPosition curPos Models ER1Position eriController CurrentData return curPos ToResponseString else if request Equals erlIMUposition Combine ER1 Position and IMU Angle Models ER1Position curPosER1 Models ERlPosition erlController CurrentData Models ER1Position curPosIMU Models ER1Position positionAlgo CurrentData Models ER1Position curPos new Models ERlPosition curPosER1 XPos curPosER1 YPos curPosIMU Angle return curPos ToResponseString else if request Equals getWiFiPosition Use WiFi x y and IMU yaw for heading angle Models ER1Position curPosWiFi Models ER1Position wifiSensor CurrentData if imuSensor isUpdating Models iMUData curIMUData Models IMUData imu
13. Values vary between 90 92 cm While the robot is turning these values do not change considerably Device placed on the front side of robot Values vary between 84 89 inches which is between 213 226 cm While the robot is turning these values go down to the range of 43 49 inches which is 109 125 cm Values are close to accurate unless they are gt 30 mor 0 6 m While the robot is turning these values go down to the range of 114 124 cm Table 13 Vex Ultrasonic Rangefinder vs FatMax TLM100 Laser Meter 70 The comparison between the Vex Ultrasonic Kit and TLM100 Laser Meter proved that using ultrasonic rangefinder does not provide as accurate readings as the laser meter does but it is much more feasible Therefore a further research to find less expensive and at the same time more accurate sensors is required Following are the two demos that are prepared to test the efficiency of Ultrasonic Rangefinder and Laser Meter on the third floor of the Atwater Kent Building around the hallway Figure 39 summarizes the measurements of position in three different cases when the sensor is mounted to the front side of the robot right side of the robot and finally the left side of the robot M IL LEER MI ir i 1 band L EXTA e l ES F JE T Ji E 1 E a 5c LL Sl LIE H TE Y as r Br TEH T 3 p TE 1 Point 14 Point 1 Direct 499 233 Direct 85 8f PING Right 92 339
14. public static SensorData NoSensorData new NoSensorData 123 public static SensorData SensorUpdatingStopped new SensorUpdatingStopped class NoSensorData SensorData override public string ToString return No Sensor Data base ToString public override object ToArray return new object No Sensor Data base Timestamp class SensorUpdatingStopped SensorData override public string ToString f return Sensor Updating Stopped base ToString override public object ToArray return new object Sensor Updating Stopped base Timestamp Appendix P 1 2 EKFState cs using System using System Collections Generic using System Ling using System Text namespace ER1LocationListener Models class EKFState SensorData private double state private double covariance private string label public static int length 3 public EKFState base this state new double length this covariance new double length length public EKFState double state double covariance base 124 this state state this covariance covariance label not set public EKFState Array state Array covariance base if state Length 5 amp amp state Length 3 throw new ArgumentException state must be length 3 or 5 Vestae else if covariance Length 25 amp amp covariance Length
15. 5 Build Ground Truth Plot ground time ground data 3 Times 29 9 23 18 12 14 9 88 170 50 80 121 71 42 85 41 29 57 30 26 50 5 start x 2025 start y 3924 y stop 3924 3942 3915 3978 3978 3942 3942 3960 3960 3960 3960 3330 1971 1971 1944 1944 2376 2376 2664 2664 3924 3942 x stop 2025 1980 1950 1908 1908 1863 1863 1602 738 730 720 720 720 720 2187 2187 2205 2205 2205 2205 2178 2178 fe 3 find index that is at location 4 and 8 temp index 1 time index 1 loc index zeros length Times 1 1 for i 1 length x pos temp ekf times scale i ekf times scale temp index if temp gt Times time index loc index time index i temp index i time index time indext1 end if time_index gt length Times break end end loc index length x stop num_points X ground zeros num points l y ground zeros num points 1 x ground 1 start x y ground 1 start y 85 store stop locations in ground truth for i 1 length x stop x ground loc index i x stop i y ground loc index i y stop i end for i 1 length x stop if i 1 x diff x ground loc index i x ground 1 y diff y ground loc index i y ground 1 startp 2 endp loc index i 1 diff points endp startp else x diff x ground loc index i x ground loc index i 1 y diff y ground loc index i y ground loc index i 1 startp loc index i 1 1 endp loc index i 1 diff point
16. Array covariance new double state Length state Length Array dummy2 new double state Length state Length Get Initial State Covariance from Matlab using WiFi and Magnometer 164 try lock matlab f string repsonse matlab Execute Initialize State matlab GetFullMatrix x base ref state ref dummy matlab GetFullMatrix P base ref covariance ref dummy2 catch Exception ex Console WriteLine EKF Init State error ex Message matlab failed so used hard coded values state new double 0 O 0 0 0 0 0 covariance new double 3 3 covariance SetValue 5 0 0 0 x covariance SetValue 5 0 1 1 y covariance SetValue 1 2 2 heading covariance SetValue 1 3 3 velocity covariance SetValue 1 4 4 angular velocity tj Create new EKF State newState new EKFState state covariance newState Type Initialize State Update the EKF s current state currentState State newState State currentState Covariance newState Covariance currentState Type newState Type Save new EKF State ReceivedData Add newState Display updated EKF state covariacne if new data was recieved and the callback is defined pass new data to callback if newState null amp amp SensorUpdateReceived null SensorUpdateReceived newState else Otherwise there was NoSensorData o
17. public FrameGrabTask CameraPanel cp this cp cp public void run URL url null try url new URL http tipAddress current jpg catch MalformedURLException e TODO Auto generated catch block e printStackTrace Image img Toolkit getDefaultToolkit createImage url img setAccelerationPriority 1 MediaTracker mt new MediaTracker cp mt addImage img 0 try mt waitForAll catch Exception ex System out println ex getMessage mt removelmage img System out println Update frame cp timer schedule new FrameGrabTask cp cp interval cp updateFrame img 108 Appen public void updateFrame Image newImg this img newImg this repaint this repaint img flush public void paint Graphics g super paint 9 this setSize this img getWidth null this img getHeight null Draw image centered in the middle of the panel if this img null g drawlImage this img 10 10 this imgX imgY this paintComponent dix 0 3 2 CommandPanel java package views import import import import public javax swing java awt event java awt models Robot class CommandPanel extends JPanel private Robot robot private JLabel lblCommand private JLabel lblResponse private JTextField txtCommand private JTextField txtResponse private JButton btnSendCommand public CommandPanel Rob
18. 0 1 numMags 10 sampleHead zeros numMags 1 magdata zeros numMags 3 for i 1 numMags Get Real Time Data pause 1 mag y inertialData 1 8 mag x inertialData 1 7 mag offset 17 pi 180 3 magnetic heading offset is 17 degrees sampleHead i 1 atan2 mag y mag x mag offset magdata i mag x mag y inertialData 1 9 end mag x mean magdata 1 mag y mean magdata 2 headAvg atan2 mag y mag x tmag offset headCov 10 pi 180 2 cov sampleHead disp mag x disp mag y disp headAvg 180 pi disp sampleHead 180 pi ox disp magdata disp mean magdata 1 3 disp mean magdata 2 3 disp mean magdata 3 oo disp headAvg 180 pi posAvg 1 posAvg 2 headAvg eye 3 3 1 1 posCov 1 2 2 posCov 2 3 3 headCov ig 1G fd fd X 83 Appendix K Angle Clip MATLAB Codes function new heading Angle Clip curr heading This function basically clips the heading to the nearest multiple of 90 degree If it is outside of the range of clipping then it is default to its original value Specifies the range for clipping range 20 These boundaries are all multiples of 90 upperbound 0 lowerbound 0 3 Finds the two values that current heading is between while abs curr heading gt upperbound upperbound upperbound 90 lowerbound upperbound 90 end 3 Finds the distance between heading and the upper an
19. Create WiFi Data object from the Matlab script s output variables double xPos double position GetValue 0 double yPos double position GetValue 1 newData new ER1Position xPos yPos SensorData timestamped automatically with current time ReceivedData Add newData Store this recevied sensor data along with the rest of the past data catch System Runtime InteropServices COMException comEx Catched MATLAB Exception Stop received updates from IMU since Matlab Exception occured stopUpdating Show user the exception and ask whether to continue receiving updates string message Message comEx Message n message Data newData n message Continue receiving updates from WiFi Controller string caption WiFi Controller Matlab Exception DialogResult result MessageBox Show message caption MessageBoxButtons YesNo Start updates back up if user chooses if result Equals DialogResult Yes startUpdating else newData SensorData SensorUpdatingStopped Update complete so allow further sensor updates to be trigger isUpdating timer Start if new data was recieved and the callback is defined pass new data to callback if newData Equals SensorData NoSensorData amp amp this SensorUpdateReceived null SensorUpdateReceived newData else Otherwise there was NoSensorData or no
20. LI Right 39 SS Figure 38 Laser Readings and Ultrasonic Sensor Readings http img705 imageshack us img705 4767 laserreadings gif http img20 imageshack us img20 2941 sensorreadings gif 71 Laser vs Ultrasonic Sensor Readings Point 1 Point 2 Point 3 Point 4 Point 5 Point 6Point 7Point 8Point 9 Point Point Point Point Point Left Laser Left Ultrasonic Right Laser 10 Right UltPasoni 3 14 Figure 39 Laser vs Ultrasonic Sensor Readings The results of the plot point out that when the laser tool or ultrasonic sensor is mounted onto the front side of the ER 1 robot the measurements are not very steady The ultrasonic rangefinder has more steady values though Furthermore the direct measurements of laser tool and ultrasonic rangefinder are not close to each other While mounted on left or right both of the sensors perform accurately and steadily The left and right side measurement values are very close to each other and they do not vary by an importance margin throughout the movement of the robot In this case new methods to increase the direct measurement precision should be analyzed 72 Ultrasound Sensor Applications The ultrasound sensor detects the distance of the closest object to the robot by transmitting echo waves Once an obstacle is detected the echo waves travel back to the sensor and the distance they travel becomes the distance of the closest object The working mechanism of the ultra
21. mapArea setLayout new BoxLayout mapArea BoxLayout Y AXIS nG C4 Slider zoomSlider mapPanel getZoomSlider CheckBox detailView mapPanel getDetailView C4 apArea add zoomSlider apArea add detailView apArea add mapPanel getHeadingLabel apArea add mapPanel ainArea add mapArea 555559 ainArea add tabPane content add mainArea setSize 1024 768 setVisible true Update Robots Location every second using ER1 Control Panel TimerTask robotLocationController new RobotLocationController robot mapPanel logger Timer timer new Timer timer schedule robotLocationController 0 1000 96 Appendix 0 2 2 ER1Controller java package controllers import java sql Time import java util Date import java util TimerTask import models Robot import views MapPanel public class ER1Controller extends TimerTask private MapPanel mapPanel private Robot robot private LoggerController logger private LocationHardwareController lhc public ER1Controller Robot robot MapPanel mapPanel LoggerController logger f this robot robot this mapPanel mapPanel this logger logger lhc new LocationHardwareController 130 215 173 219 public boolean updateRobotPosition Robot robot mapPanel getRobot if robot connected return false send robot command to get position parse update position and redraw String positio
22. 0 003207814646 0 001513966359 0 26818159222 0 06062751263 0 57857650518 155 9537353515 0 65752309560 4 39401292800 22 47 52 9531250 0 089160658419 0164139556884 9819113731384 0105592556873 0 01227180566 0 02936150133 026527407765 107400093188 40 58561694622 155906906127393 0166451251506 4 40074157714 22 47 52 9375000 0100015200674 071280521154 9 774773597717 0 02261146716 0 00654896255 000295026297 026456522941 009030662695 059694005012 1559115142822 0165624284744 438488435745 22 47 52 9375000 0 083629921078 067806035280 9 799072265625 004664527590 0 007395897991 001389094481 026354235410 0 03102693945 058263248205 1556946533203 65014111995 4 39559555053 2247529216750 0 076379835605 0 63649404287 9 799295425415 001314600605 0012118990395 004502701014 02713245749 005213095620 057933419942 1559903106689 D66111761331 4 38949971063 2247529218750 0 090850912034 0 71280705928 9 773171424865 002010387927 001416095567 0006991452537 1026987762548 005196991352 057909256219 1559770355224 067729793058 4 40072345733 2247 52 9062500 1094527259469 266511142253 3795082092295 0 04500834271 1 nngsss4g9n53 008223873563 N 26494875550 0Z86A180952 1 58706724643 155 9074249267 N 66223263740 sA ANBRGR79975 22415 Figure 21 Robot location server showing the different algorithms Next
23. 155 9184265196 0 70046943426 4 3648247718811 22 47 53 3906250 0 065393649041 0 65069549415 9817909240722 0 03143828362 0 016243522986 0 00286977598 027073931298 0 05301316317 0 57994228601 1553758605957 069358867406 4 38313102722 22 47 53 1875000 0 087229296565 0 70551383495 9 780616760253 0 02971249260 0 01202883571 002505669232 02700142501 005694114044 058054053783 155 9395174560 070053112506 4 37474956049 _ 22 47 53 1718750 0 101840570569 0 67243063449 3791222572326 0 04682341217 001002750874 0 01722891256 025570487022 0 07755859947 0 58499449491 155 8799591064 0 69046014547 4 37141752243 2247531718750 0 098273642361 0 67387251281 9 798693656921 0 02263160236 0 01852328702 0 00628598360 026494063228 0 07747042924 0 58475130796 1558949279785 0 68263375759 4 37017059326 22 47 52 9687500 0 069696469068 067227989435 9 768152236938 0101917412488 0 01412854902 0 00076173123 0 26696461439 008851014922 058251571655 155 8966827392 0 66611808538 4 3811821937561 22 47 52 9531250 0 054239928722 0 64680767059 9 799781799316 0 02146290801 0 02721910551 000518736662 026930563505 006095042202 0 57833057641 155 9695434570 067335050000 4 39456796646 2247529531250 0 081526793539 0 70897448062 9 754996299743 0 02689004448
24. Check connection with ER1 Telnet Remote API if connected string message Cannot connect to ER1 Telnet Remote API n message Do you want to enter simulation mode string caption ER1 Controller Connection DialogResult result MessageBox Show message caption MessageBoxButtons YesNo Don t start updating the ER1 Controller if no connection to Telnet Remote API and simulation mode was declined if result Equals DialogResult No return 142 Start Updating the Position using the Algorithm base startUpdating Forml mainForm Forml Forml ActiveForm mainForm SafeRenameButton btnStartUpdating Stop Updating ER1 public override void stopUpdating f Stop Updating the ER1 Remote API base stopUpdating Forml mainForm Forml Forml ActiveForm mainForm SafeRenameButton btnStartUpdating Start Updating ER1 Read the current values from the ER1 and store it override public void doUpdate object sender System Timers ElapsedEventArgs e string response ERlPosition position null If connected to ER1 over Telnet if connected Send command to ER1 Telnet Remote API and get response response sendCommand position Parse response to get ER1Position try position ERlPosition Parse response Handle Invalid ER1 Position Response from ER1 Remote API catc
25. Console WriteLine IMU Change in heading is Inf or NaN dh total so forcing to ZERO dh total 0 Console WriteLine IMU Change in heading dh total radians using length lastCount this data Count 1 IMU samples over dt total ms Set the index of the last IMU value used in EKF lastCount this data Count lock EKFAlgorithmClass CurrentState EKFAlgorithmClass Time Update erlController getTotalDisplacement dh total EKFAlgorithmClass Time Update erlController getDisplacement erlController R eceivedData Count 1 dh_total ParameterizedThreadStart pts new ParameterizedThreadStart EKFAlgorithmClass Time Update System Threading Thread thread new System Threading Thread pts thread Priority ThreadPriority Highest thread Start dh total dt total public void ExportToMatlab LEM lock matlab int rows ReceivedData Count int cols IMUData CurrentData ToMatlabArray Length matlab Execute imudata zeros rows cols sa cat Yar for int i 0 i lt ReceivedData Count itt Array rowData IMUData ReceivedData i ToMatlabArray Array dummy new double rowData Length matlab PutFullMatrix row base rowData dummy matlab Execute imudata 1H 1 1 rowData Length t row matlab Execute save imudata mat imudata 151
26. IMUData cs using System using System Collections Generic using System Ling using System Text namespace ERlLocationListener Models class IMUData SensorData Accelerometer Data private double AccelX private double AccelY private double AccelZ Gyroscope Data private double GyrX private double GyrY private double GyrZ Magnetic Compass Data private double MagX private double MagY private double MagZ Euler Orientation private double Yaw private double Pitch private double Roll public IMUData Array accel Array gyr Array mag base f AccelX double accel GetValue 0 AccelY double accel GetValue 1 AccelZ double accel GetValue 2 GyrX double gyr GetValue 0 GyrY double gyr GetValue 1 GyrZ double gyr GetValue 2 MagX double mag GetValue 0 MagY double mag GetValue 1 MagZ double mag GetValue 2 public IMUData Array inertialData Array eulerAngle base 131 AccelX double inertialData GetValue 0 AccelY double inertialData GetValue 1 AccelZ double inertialData GetValue 2 GyrX double inertialData GetValue 3 GyrY double inertialData GetValue 4 GyrZ double inertialData GetValue 5 MagX double inertialData GetValue 6 MagY double inertialData GetValue 7 MagZ double inertialData GetValue 8 Yaw double eulerAngle G
27. PixelsPerCM int line getEndPoint getY PixelsPerCM Appendix 0 3 8 RobotView java package views import javax swing JComponent import java awt Color import java awt Graphics import models public class RobotView extends JComponent private Robot robot private double PixelsPerCM 0 25 private double PixelsPerCM 20 0 180 0 public RobotView Robot robot f this robot robot public Robot getRobot return robot private int CMToPixels double cm return int cm PixelsPerCM public void setZoom double zoom PixelsPerCM zoom public void paint Graphics g Get position and direction facing 121 Point location robot getLocation int angle int robot getAngle Convert from real distance to pixels for rendering int xPixels CMToPixels location getX int yPixels CMToPixels location getY Draw Direction Facing int angleWidth 60 int width CMToPixels 200 int height CMToPixels 200 g setColor Color BLUE g drawArc xPixels width 2 yPixels height 2 width height angle angleWidth 2 angleWidth g fillArc xPixels width 2 yPixels height 2 width height angle angleWidth 2 angleWidth true true Draw Robot at location width CMToPixels robot getWidth height CMToPixels robot getHeight g setColor Color GREEN g draw3DRect xPixels width 2 y
28. currentState State3 Covariance currentState Covariance3 dummy new double State Length dummy2 new double 5 5 dummy2 new double 3 3 Get displacement in both Position ER1 and Heading IMU Displacement new double deltaPosition deltaHeading dummy3 new double Displacement Length Use matlab to Update Current EKF State and Covariance using Displacement string response try lock matlab Put required values for MATLAB workspace EKF Time Update into matlab Execute clear global x matlab PutFullMatrix x matlab PutFullMatrix P dummy2 matlab PutFullMatrix u dummy3 168 base State dummy base Covariance base Displacement Prediction yx dummy dummy response Run MATLAB Script to do the EKF Time Update response matlab Execute x P Time Update x P Get the Updated State and Covariance from MATLAB matlab GetFullMatrix x base matlab GetFullMatrix P base ref State ref ref Covariance ref Console WriteLine EKF Time Update Successful Create new EKF State tj newState Type KFState newState new EKFState State Covariance Time Update dPos deltaPosition amp dHead deltaHeading Catched MATLAB Update the EKF s current state urrentState Type n
29. matlab null matlab return false Execute run C Program Files MATLAB R2006b toolbox System Windows if msg Contains return true return false ERToolkits erClose Forms MessayeBox Show msg closed succesfull public bool update camera frame f string msg lock this try Request Camera Image from Server matlab Fxecute addpath C Program Files MATLAB R2006b toolbox ERToolkits msg matlab Execute img erGetImage If erGetImage response display it if msg Trim Equals Console WriteLine msg Return false if error getting image in Write msg msg Contains erInit msg Contains server error mat msg Contains error in return false image to file ab Execute imwrite img esNourrent jpg e If matlab image write has response display it Lf msg n Trim Equals Console WriteLine msg 174 gaten Exception ex Console WriteLine CAM Excepct return true string getResponse string request HttpLis request request Replace 20 Space with a space if request Equals favicon ico Declare the response for the reques string response return ion ex Message tenerContext context replace ASCII code for Wit Display Status Page cont
30. sensor as well as the post processing engine The above illustrates our overall algorithm Extended Kalman Filter for our sensor integration Basically the IMU and Odometer will be sampled at 100 Hz and 5 Hz respectively Next their data will be fed into the Displacement Computer where the change in position and heading are tracked This block runs at a frequency of 1 Hz Our research also shows that the ERI velocity readings are very good However the angle tracking ability of the IMU is very limited Hence the sensor that we will use to correct the drifts is Wi Fi Localization using the 27 Page Kernel Method The majority of these next few sections describe the integration of the Kernel Method and Extended Kalman Filter in details 3 1 Kernel Method for Wi Fi Localization We implemented a Wi Fi localization method that is very similar to the one previously described by Roos 6 involving the use of the Gaussian Kernel The entire flow chart of our localization system is attached on the appendix Appendix B The first step in building our Wi Fi localization system is to start with constructing the database which is illustrated below START Read in RSS in dB DATABASE P 85 74 t m n l m access point n sample l location Compute Sum of Squared Errors for Each Sample and Location SSE n D gt P t Ck n D k 1 Figure 12 Diagram for constructing the database and computing t
31. true In Control else updateControlLabel false Click Here To Control public void updateControlLabel boolean inControl String state if inControl amp amp robot connected controlLabel setBackground Color GREEN controlLabel setText state controlLabel requestFocus else if robot connected controlLabel setBackground Color RED controlLabel setText ER1 API Not Connected else controlLabel setBackground Color RED controlLabel setText state public void displayNotInControl controlLabel setBackground Color RED if robot connected controlLabel setText ER1 API Not Connected else controlLabel setText Click Here To Control 103 Appendix 0 2 5 RobotLocationController java package controllers import java util ArrayList import java util TimerTask import models import views public class RobotLocationController extends TimerTask private MapPanel mapPanel private Robot robot private LoggerController logger public Robotl LoggerController LocationController Robot robot MapPanel logger this this this robot mapPanel logger robot mapPanel logger public boolean connected if robot connected return false String resp resp trim if resp startsWith CONN robot getClient sendCommand is ECT ED return true
32. x noise u 1 0 15 if x noise lt 0 5 x noise 1 end 3 System noise covariance Q eye 3 3 seye 5 5 Q 1 1 x_noise 2 x 1 cm Q 2 2 x_noise 2 y 1 cm Q 3 3 0 5 pi 180 2 heading 0 000001 radians Force Angular Displacement to 0 if less than 3 degrees if abs u 2 lt 3 pi 180 u 2 0 O34 3 L03 2 end 3 Current State xP x 1 3 x Position yP x 2 3 y Position Phi x 3 3 Heading dPos u 1 3 Change in position dPhi u 2 Change in Heading x 1 xP dPos cos Phi predict new x lt dPhi x 2 yP dPos sin Phi predict new y lt dPhi x 3 Phi dPhi predict new heading Clip Heading Angle if dPhi 0 x 3 Angle Clip x 3 180 pi pi 180 nd Build the Stystem matrix A This is the jacobian matrix of the prediction function f A eye 5 5 D D D oe oe oe oe oe oe D A 1 3 dPos sin Phi A 2 3 dPos cos Phi A 1 4 cos Phi A 2 4 sin Phi A 3 5 0 eye 3 3 1 3 dPos sin Phi 2 3 dPos cos Phi P A P A Q 8l Appendix I EKF Measurement Update MATLAB Codes function x P WiFi Measurement Update x P z WiFi Measurement covariance R eye 2 2 R 1 1 15 2 WiFi x 0 5cm R 2 2 15 2 WiFi y 0 5cm o Build the measurement matrix H zeros 2 3 H TIY Bee H22 2 d o measurement function hx 1 1 x 1 x Position hx 2 1 x 2 y Position
33. 1 1 y covariance SetValue 1 2 2 heading covariance SetValue 3 3 velocity lal covariance SetValue 1 4 4 angular velocity KF State m newState new EKFState state covariance newState Type Initialize State Update the EKF s current state currentState State newState State currentState Covariance newState Covariance currentState Type newState Type 166 Save new EKF State ReceivedData Add newState Display updated EKF state covariacne if new data was recieved and the callback is defined pass new data to callback if newState null amp amp SensorUpdateReceived null SensorUpdateReceived newState else Otherwise there was NoSensorData or no callback was defined Console WriteLine EKFController data newState SensorDataReceived SensorUpdateReceived n public static void ExportToMatlab try lock matlab matlab Execute xSaved zeros ReceivedData Count 3 matlab Execute pSaved zeros ReceivedData Count Wp oro Jj matlab Execute state info cell 2 for int i 0 i lt ReceivedData Count i Array State EKFState ReceivedData i State Array Covariance EKFState ReceivedData i Covariance Array dummy new double State Length Array dummy2 new double State Length e State Length
34. ER1Positions x y heading velocity timestamp to Matlab public void ExportToMatlab try MLApp MLAppClass matlab new MLApp MLAppClass lock matlab for int i 0 i lt ReceivedData Count i Array rowData ERlPosition ReceivedData i ToMatlabArray Array dummy new double rowData Length matlab PutFullMatrix row base rowData dummy matlab Execute erldata i 1 1 rowData Length row matlab Execute save erldata mat erldata catch System Runtime InteropServices COMException comEx Catched MATLAB Exception Display Error string message Error exporting ER1 Data to Matlab n message Exception comEx Message n MessageBox Show message 144 Sends a command to the ER1 Control Panel s Telnet Remote API and Returns its resposne public string sendCommand string command Make sure we are connected to the ER1 Telnet Remote API if connected return NOT CONNECTED Send the command through telnet to the ER1 Remote API telnet WriteLine command Read the response of the command from telnet string response telnet Read Return the response return response Returns Telnet Connection status public bool connected return telnet null amp amp telnet IsConnected IP Address of the Laptop runn
35. Override public void run TODO Auto generated method stub updateRobotPosition mapPanel repaint Appendix O 2 6 LoggerController java package controllers import import import import import java java io File security Timestamp java java sql Time util ArrayList javax swing JTextArea 105 import models public class LoggerController private File file private boolean logging private boolean replay private JTextArea txtLogArea private ArrayList lt PositionLog gt robotLog public LoggerController f file null logging false dit replay false txtLogArea null robotLog new ArrayList PositionLog public ArrayList PositionLog getPositionLog return robotLog public boolean isLogging return logging public void clearLog robotLog clear if txtLogArea null txtLogArea setText X tY tangle ttime tvelocity n public void startLogging I stopReplay logging true clearLog public void stopLogging logging false public boolean isReplay 106 return replay public void startReplay replay true stopLogging public void stopReplay replay false public void logPosition Point location robotTrace addPoint location d public void logLocation double x double y double angle Get current t
36. RFID Assisted Indoor Localization and Commuication for First Responders Tech 2005 Web http www antd nist gov wctg RFID IS ART06 Assisted LocCom pdf Se Stephen David Lowe and Jim Little Vision based Mobile Robot Localization and Mapping using Scale Invariant Features Tech 2000 Web lt http citeseerx ist psu edu viewdoc download doi 10 1 1 62 2166 amp rep rep1 amp type pdf gt Frank Korbinian Bernhard Krach Noel Catterall and Patrick Robertson Development and Evaluation of a Combined WLAN amp Inertial Indoor Pedestrian Positioning System Working Paper 2009 Print Rekleitis loannis M A Particle Filter Tutorial for Mobile Robot Localization TR CIM 04 02 Tech 2003 Web lt http www cse buffalo edu peter refs DataAssimilation GenRefsTutorials particletutorial pdf gt Pahlavan Kaveh Xinrong Li and Juha Pekka Makela Indoor Geolocation Science and Technology Tech Feb 2002 Web lt http www cwins wpi edu publications 00983917 pdf gt Roos Teemu Petri Myllymaki Henry Tirri Pauli Misikangas and Juha Sievanen A Probabilistic Approach to WLAN User Location Estimation International Journal of Wireless Information Networks 9 3 2002 Web lt Http www springerlink com content rdtrqOrpptfl geby fulltext pdf gt Riisgaard Soren and Morten Rufus Blas SLAM for Dummies A Tutorial Approach to Simultaneous Localization and Mapping Rep Print Ralf Solomon Matthias Schneider and Daniel Wehden
37. RobotTrace robotClient new ER1Client 192 168 1 2 USES OLD ER1 use ER1 Location Sensor Manager Server robotClient new LocationSensorManagerServer 192 168 1 2 connected false Use new ER1 Location Sensor Manager Server 89 REMOVING OLD ER1Client public ER1Client getClient x public LocationSensorManagerServer getClient return robotClient public RobotTrace getTrace return robotTrace public boolean connected return connected public boolean connect String ipAddress robotClient setIPAdress ipAddress connected robotClient Connected return connected REMOVING OLD ER1Client public boolean connect connected robotClient connect return connected public void disconnect robotClient disconnect connected false kf public double getWidth f return width public double getHeight return height public Point getLocation return location 90 public double getX f return location getX public double getY return location getY public double getAngle return angle public void updateLocation double x double y double angle if this connected Update x y coordinates in cm and anglein degrees location setX x initialOffset getX location setY y initialOffset ge
38. ToString Store this recevied sensor data along with the rest of the past data ReceivedData Add newData catch Exception ex Catched MATLAB Exception Stop received updates from IMU since Matlab Exception occured stopUpdating string message Sonar Controller Ex ex Message An message Trace ex StackTrace An MessageBox Show message newData SensorData SensorUpdatingStopped Forml form Forml Forml ActiveForm form SafeRenameButton btnStart Stop Updating Sonar if new data was recieved and the callback is defined pass new data to callback if newData Equals SensorData NoSensorData amp amp this SensorUpdateReceived null SensorUpdateReceived newData else Otherwise there was NoSensorData or no callback was defined Console WriteLine IMUController data newDatat SensorDataReceived SensorUpdateReceivedt Nn 159 public void ExportToMatlab tr f lock EKFAlgorithmClass Matlab f for int i 0 i lt ReceivedData Count i Array rowData SonarData ReceivedData i ToMatlabArray Array dummy new double rowData Length EKFAlgorithmClass Matlab PutFullMatrix row base rowData dummy EKFAlgorithmClass Matlab Execute sonardata i 1 1 rowData Length row EKFAlgorithmClass Matlab Execute save sonardata mat sonstddta
39. achieved a certain amount of velocity and angular velocity The following set of equations basically outlines the dead reckoning algorithm 0 0 1 A V lv x TV AX V x cos 0 Eq 12 0 7 Pe Gy This equation is based on the assumption that the velocity of the robot is not accurate in X and Y directions particularly when the robot turns However the total velocity will give a more precise measurement Using the change in angle and the total velocity we can simply compute the change in x direction and the change in y direction The angular velocity is simply added to the previous heading to achieve the new heading because the angular velocity has a timing interval of one second The figure below illustrates the results of the dead reckoning algorithm that we developed from our set of equations As shown in the plot below once the angle displayed an incorrect measurement the entire algorithm went off course 44 Page ER1 amp IMU vs Ground Truth Figure 23 Performance of the dead reckoning algorithm green vs ground truth purple Furthermore once the angle is incorrect the algorithm cannot correct itself This is especially shown on the above plot in which as soon as the angle went off the entire algorithm fails There are several areas on the above plot in which the tracking went off the map which is a very poor estimate The first performance test we used to analysis this algorithm s data is the timing error
40. algorithm the initial position and heading of the object is estimated by data based on Wi Fi RSS and a magnetometer Then all the readings from the various sensors are combined using the EKF Our algorithm is largely based on 5 In the original algorithm a shoe mounted magnetometer is used and passed into a ZUPT filter along with other IMU sensor readings to yield cleaner and smoother results The ZUPT is not used in our design since the 12 Page odometer mounted to the robot is used to achieve the velocity of the robot Another reason is that the motion of the robot is smoother than that of human movement which results in far less corruption of noise in the raw velocity data Other than the difference in the velocity tracking another key difference is the angle tracking Besides using a gyroscope to track the changes in angle we compensated the drift in the gyroscope using ultrasound sensors We also realized that the ultrasound sensors are less prone to drift than the magnetometer Hence the application of the ultrasound sensors results in far better angle tracking and thus a much more accurate localization algorithm Another contribution to 5 is that we developed the user interface that can implement indoor geolocation in real time This allows for better robotic applications involving indoor localization Robot Server Robot Client Remote Communication Internet Sensor Fusion Algorithms l j Dead Reckoning Inertial Wi Fi
41. analysis which is shown below 45 Page Timing Error Analysis of ER1 amp IMU Algorithm 25 15 Error in meters 10 0 200 400 600 800 1000 1200 Time in s Figure 24 Timing error analysis of the dead reckoning algorithm The timing error analysis shows that the overall performance of the dead reckoning algorithm is decent during the first 200 seconds However as soon as the robot changes its orientation such that the IMU s drift caused the overall heading to be inaccurate the robot can no longer find its own position Since the robot can never recover from the heading distortion it is impossible to track the robot s position thereafter Although there is a small area where the error was reduced to below 2 meters at approximately time equals 850 seconds we believe that it was simply coincidence that the robot s path happens to cross the ground truth data at that time Using the timing error analysis we conducted a CDF test to find the percent error of the robot s position versus the ground truth position 46 Page CDF of ER18IMU 0 9 0 8 0 7 0 6 0 5 Probability 0 4 0 3 0 2 0 1 0 5 10 15 20 25 Error in Meters Figure 25 Cumulative density function of the dead reckoning algorithm From the CDF plot we can also see that the ERI amp IMU only algorithm produces really poor position tracking The conclusion is that the IMU amp ERI will require Wi Fi Localization to fix its drift because the above resu
42. based on these techniques use signal features such as Time of Arrival TOA Angle of Arrival AOA and Received Signal Strength RSS to estimate an object s location However due to the inaccuracy of Wi Fi localization other sensors must be incorporated to compensate for the error In recent years an Inertial Measurement Unit IMU and other sensors are integrated with Wi Fi to 8IPage provide better indoor position estimates Hence the topic of our Major Qualifying Project is to design an effective indoor geolocation scheme for robotic applications based on the fusion of Wi Fi localization inertial sensors and rangefinder sensors 1 1 Motivation Traditionally indoor localization algorithms were mainly developed for tracking people and assets as described in 1 However with the emergence of robotics more and more applications involving automated tasks require precise localization of the robot The main motivation behind robotic indoor localization is that such an application allows robots to be location aware With modern software techniques a robot can be automated to perform certain Jobs inside a building as long as the location is known Figure 1 Indoor robotic applications requiring location awareness http dvice com archives 2008 10 i robot maid to php 9 Page For example a robot can serve as a maid similar to the one on the above figure but this would not be possible without knowing the exact location of
43. callback was defined Console WriteLine WiFiController data tnewDatat SensorDataReceived SensorUpdateReceivedt n DateTime endtime DateTime Now Console WriteLine WiFi Update to finsih took endtime TimeOfDay Subtract start TimeOfDay n kj 155 public void ExportToMatlab f try f lock matlab for int i 0 i lt ReceivedData Count itt Array rowData ERlPosition ReceivedData i ToMatlabArray Array dummy new double rowData Length matlab PutFullMatrix row base rowData dummy matlab Execute wifidata i 1 1 rowData Length row matlab Execute save wifidata mat wifidata catch System Runtime InteropServices COMException comEx Catched MATLAB Exception Display Error string message Error exporting WiFi Data to Matlab n message Matlab Command scriptName n message Exception comEx Message n MessageBox Show message public void ReceiveWiFiData WiFiData wifiData lock thig this wifiData wifiData if wifiDB NeedsSample wifiDB Add this CurrentWiFiData if this isUpdating this doUpdate null null public WiFiData CurrentWiFiData get lock wifiData return wifiData set lock wifiData wifiData value 156 public MLApp MLAppClass Matlab get retu
44. double dT 0 if last null f int i data IndexOf current it 3 0 last IMUData data i 1 if last null amp amp last Equals SensorData NoSensorData ar current Timestamp TimeOfDay Subtract last Timestamp TimeOfDay Milliseconds AT 1000 0 140 else f dT this TimeInterval 1000 0 return dT Get the displacement in position between the position at index sample and the position previous to that sample public double getTimeDifference int sample if sample lt 2 sample gt this data Count return 0 SensorData current SensorData data sample SensorData last SensorData data sample 1 double dt current Timestamp TimeOfDay Subtract last Timestamp TimeOfDay Milliseconds return dt override public string ToString string str TimeInterval is interval ToString milliseconds n str Polling Status updating Updating NOT Updating n str Last Received Value CurrentData null NO DATA CurrentData ToString n return str Appendix P 2 2 ER1Controller cs using System using System Collections Generic using System Ling using System Text using System Windows Forms using ERlLocationListener Models namespace ERlLocationListener Controllers This class interfaces with the ER1 Control Panel Software s Telnet Remote A
45. get return Heading 180 0 Math PI set Heading value 180 0 Math PI public double Velocity get return state 3 set state 3 value 126 public double AngVelocity f get return state 4 set state 4 value public double XCov get return covariance 0 0 set covariance 0 0 value public double YCov get return covariance 1 1 set covariance 1 1 value public double HeadingCov get return covariance 2 2 set covariance 2 2 value public double VelocityCov get return covariance 3 3 set covariance 3 3 value public double AngVelocityCov get return covariance 4 4 set covariance 4 4 value public string Type get return label set label value public string ToResponseString return OK XPos ToString YPos ToString HeadingDegrees ToString NO override public object ToArray return new object XPos YPos HeadingDegrees Velocity AngVelocity 127 XCov YCov HeadingCov VelocityCov AngVelocityCov Timestamp TimeOfDay ToString label Appendix P 1 3 ER1Position cs using System using System Collections Generic using System Ling using System Text using System Windows Forms namespace ERlLocationListener Models class ER1Position SensorData private double xPos ERI x p
46. gotAllWiFiSamples value public WiFiDB TextBox txtSamplesRead Forml form f name not set samples new ArrayList LEM f matlab new MLApp MLAppClass catch Exception ex Console WriteLine Cannot Load Matlab samplesToRead 0 this txtTotalSamplesRead txtSamplesRead mainForm form public bool NeedsSample get return SamplesRead lt SamplesToRead 161 public void Add WiFiData wifiData f samples Add wifiData samplesRead UpdateTotalSamplesReadTextBox if NeedsSample amp amp gotAllWiFiSamples null gotAllWiFiSamples this public void Clear samples Clear samplesRead 0 samplesToRead 0 UpdateTotalSamplesReadTextBox private void UpdateTotalSamplesReadTextBox mainForm SafeSetText this txtTotalSamplesRead TotalSamplesRead ToString public string Name get return name set name value public int TotalSamplesRead get return samples Count public int SamplesRead get return samplesRead set samplesRead value public int SamplesToRead get return samplesToRead set samplesToRead value public int findMaxAPs int maxAPs 0 foreach WiFiData wifiData in samples if wifiData Length gt maxAPs maxAPs wifiData Length 162 findMaxAPs return maxAPs public void save bool
47. i string value double value string str Time Recorded 4 this Timestamp TimeOfDay ToString Environment NewLine for int i 0 i lt this Length i Str MAC Nt 4 thas i 0 XtRSSI this i 1 dBm n return str override public object ToArray f object array for int i 0 array i return array public object ToMatlabArray 135 MACs i new object this Length i this Length i RSSIs i object array for int i 0 new object this Length 2 lt this Length i 0 1 array i MACs i array i return array Appendix P 1 6 SonarData cs using System using System Collections Generic using System Ling using System Text namespace E RlLocationListener Models class SonarData SensorData Ultrasonic Rangefinder Data private double front Dista private double back Dista private double left Dista private double right public SonarData Array distanc double double double double distances distances distances distances front back left right ML tt Nt fe public SonarData double front right base this front front this back back this left left this right right public SonarData string front right base right right Trim this front Double Parse th
48. imuSensor stopUpdating else Not Updating so startUpdating imuSensor startUpdating private void btnStartUpdatingPosition Click object sender EventArgs e Stop updating position if already updating if positionAlgo isUpdating positionAlgo stopUpdating else Otherwise start updating the position using the algorithm Start getting ER1Position data every TimeInterval positionAlgo startUpdating private void btnClearAll Click object sender EventArgs e Clear Stored ER1 Data in ERlController and DataGridView erlController Clear grdERlPosition Rows Clear Clear Stored IMUData in IMUController and DataGridView 185 imuSensor Clear imuSensor ClearCount dgvlIMUData Rows Clear Clear Stored WiFiData in WiFiController and DataGridView wifiSensor Clear dgvWiFiData Rows Clear Clear Stored SonarData in SonarController and DataGridView sonarSensor Clear dgvSonarData Rows Clear Clear Stored Positions and Reset the Current Position positionAlgo Clear Remove all past positions calculated from algorithm positionAlgo resetPosition Reinitialize the position to 0 0 dgvAlgorithm Rows Clear Remove all rows from the user interface Clear Stored EKF EKFAlgorithmClass Clear dgvEKF Rows Clear private void btnStartUpdatingAll Click object sender EventArgs
49. magnetometet less i eret reote eese coo ipee eee Weve dude 21 Figure 10 Ultrasonic rangefinder kit for correcting the heading esses 25 Figure 11 Overall description of the algorithms with an outline of the individual sampling frequencies of each sensor as well as the post processing engine sessesssesssssrsssresseresersseresseee 27 Figure 12 Diagram for constructing the database and computing the sum of squared errors for each sample and location using the received signal strength readings eeeseeceeneeeeeeeeeeeeeeees 28 Figure 13 Creating and normalizing the Kernel for probability estimate 29 Figure 14 Calculating the probability of the robot in each location using the Kernel 29 Figure 15 Overview of the processes involved in the Extended Kalman Filter algorithm 30 Figure 16 Architecture overview identifying the sensors as well as the server to client platform with real time remote control video stream and map display eee 35 Figure 17 Robot client side of the architecture with remote control video stream and location Mapa papaanu AA NA NA inse Aa Ba 36 Figure 18 Client interface with the video stream on the upper right control system on the lower right and the map location display on the left eese nennen 38 Figure 19 Robot server side of the architecture responsible for sampling th
50. real time Next Chapter V analyzes the results from the experiments using the system that is designed by our algorithm It follows with a comparative performance analysis which illustrates the pros and cons of each of the algorithms tested in our project Lastly Chapter VI is the conclusion of the project which also includes a brief discussion on the remaining work that can be done in the future 141Page CHAPTER II Analysis of Sensor Behaviors After careful research and analysis we decided to use Wi Fi localization based on RSS MTx Inertial Measurement Unit Evolution Robot 1 and lastly the optical sensors Each one of these sensors serves a specific function making them all crucial in our localization method Thus the purpose of the following subsections is to illustrate and explain the functions of each of these sensors 2 1 Sensing the RSS of Wi Fi Wi Hi localization has gained much popularity over the recent years Many indoor localization applications have revolved around the notion of using received signal strength measurements Although there are methods to implement Wi Fi localization using TOA AOA and RSS the most widely used algorithm involves using RSS There are typically two basic Wi Fi localization techniques that are generally used in indoor localization These two methods are the nearest neighbor method and the kernel method as suggested by 6 The nearest neighbor method basically revolves around the notion of
51. the algorithm component combines the raw data into a single 2D location with an angle of the robot s heading direction The three main algorithms implemented in the ER1 location server are the dead reckoning algorithm Wi Fi Kernel method and EKF Other algorithms can be implemented in the ER1 Locator Server which can use any of the current or past values from any sensors to produce a position coordinate and heading direction which can be passed to the client interface for end user display The ERI Location Server has been integrated with MATLAB so the actual algorithm can be implemented as a MATLAB function This allows logged sensor data to easily be transferred to and from MATLAB for both analysis and algorithm implementation The ERI Location Server triggers the positioning algorithm with the current sensor data and updates the current position with the end result 4llPage CHAPTER V PERFORMANCE ANALYSIS The purpose of this section is to depict the results we achieved using our current algorithms There are a total of three algorithms that we tested a basic algorithm that combines the ERI and IMU also known as Dead Reckoning Wi Fi Kernel Method and lastly the Extended Kalman Filter on the ERI IMU and Wi Fi Each section will illustrate the results of the above algorithms using a ground truth plot generated from actually measuring the robot s position at a specified time Additionally each section will also discuss in detail the limitat
52. the position of the robot 50 lPage 5 4 Performance using the Extended Kalman Filter for Fusion This next plot illustrates the Entire EKF algorithm and this is plotted versus ground truth The method for developing ground truth is by examining the locations in which the robot stopped as well as the corresponding time With those two parameters we can easily generate a ground truth with the assumption that the robot is traveling with linear velocity Using those points we generated the ground truth in MATLAB and plotted our EKF data against it EKF vs Ground Truth 100 150 200 250 300 Figure 29 Performance of the Extended Kalman Filter algorithm blue vs ground truth black From the plot we can see that the EKF maps the movement fairly well The problem in the accuracy however is largely due to the orientation error created from the drift of the IMU 5 IPage We have tweaked the covariance parameters of our EKF to reject as much drift as possible in order to find the robot more accurately From the EKF map we can clearly see that the angle or orientation is approximately correct The following plot better illustrates the error we obtained Timing Error Analysis of the EKF Algorithm Error in meters 0 200 400 600 800 1000 1200 Time in s Figure 30 Timing error analysis of the Extended Kalman Filter Algorithm In the figure above the EKF algorithm had an outstanding performance With the exception of the time bet
53. the robot In a sense this new notion of an automated task performing robot is the evolution of Telepresence Not only will individuals be able to meet face to face but they may also take advantage of the robot s mechanic instruments to perform a variety of tasks Other applications proposed in this field include a tour guide system where a robot can be used to give a tour in a museum and explain certain artifacts and artworks along the way 2 However due to the fact that GPS is completely inaccurate in indoor environments more and more research has been conducted on hybrid localization techniques Our Major Qualifying Project focuses on the design and implementation of a location aware multi sensor robotic platform for indoor applications by taking advantage of existing Wi Fi infrastructure In terms of current technology the integration of multiple sensors to precisely track the movement of an object in an indoor environment is a new and evolving technology Not only will our project build on existing localization methods such as Wi Fi but more importantly it offers a new approach and a different application to improve the existing localization technology further 1 2 Previous Work In the past few years Wi Fi localization has been studied in indoor location systems However Wi Fi alone does not provide the accuracy good enough to locate an object due to two main flaws The first is that Wi Fi localization has shown that it s slow responsiv
54. update stage with the gyroscope and odometer Finally due to the drift imposed by the local sensors we compensate for it during the measurement 3 An Introduction to the Kalman Filter by Greg Welch and Gary Bishop Page 11 301Page update phase using the Wi Fi Kernel algorithm The majority of the following subsections focus on explaining each of these three stages 3 2 1 Initialization Process There are many approaches to implementing this filter one of the best methods is by developing a three state filter implemented by 10 To create this filter the first step is to identify the current state x y Eq 1 The current state of our EKF will contain three elements the x coordinate y coordinate and the heading These are also the main three terms that will be altered during each phase of the EKF During the initialization stage we use the W1 Fi position estimates and the magnetometer to initialize the states First we collect 10 samples from the Wi Fi Kernel method outputs which contain the x coordinate and y coordinate Then we calculate the average and standard deviation of those measurements to be used as the initial position and position covariance The position covariance is shown below Xur 3 0 P 0 ysa 0 Eq 2 0 Dr Bl The purpose of the covariance matrix is to model the drift to determine at what point to weight in the Wi Fi measurements With high covariance values the weight of each Wi Fi measure
55. velocitySlider iparam args s7 public static void main String args TODO Auto generated method stub new ERINavigatorFrame public ER1NavigatorFrame super ER1 Navigator setDefaultCloseOperation EXIT ON CLOSE UIManager setLookAndFeel UIManager getSystemLookAndFeelClassName Container content getContentPane content setBackground Color lightGray Create the models 95 models Robot robot new models Robot Map map new Map Create the controllers LoggerController logger new LoggerController MoveController moveController new MoveController robot MapController mapController new MapController Create the views MapPanel mapPanel new MapPanel map robot mapController ConnectionPanel connectionPanel new ConnectionPanel robot CommandPanel commandPanel new CommandPanel robot ControlPanel controlPanel new ControlPanel moveController LoggerPanel loggerPanel new LoggerPanel robot logger Create the tabbed pane of views JTabbedPane tabPane new JTabbedPane tabPane addTab Connection connectionPanel tabPane addTab Command commandPanel tabPane addTab Control controlPanel tabPane addTab Logging loggerPanel Add MapPanel to left half side of the screen and the TabbedPane to the right half side JPanel mainArea new JPanel new GridLayout 1 3 JPanel mapArea new JPanel
56. 0 31370 0 14890 0 44310 0 50580 0 20790 0 70450 STD 0 00072 0 00100 0 00078 0 00125 0 00140 0 00358 0 00896 0 00765 Mean 0 03358 0 34789 0 31425 0 14721 0 44317 0 50655 0 21043 0 70085 Heading 84 48636 154 89865 131 18219 106 71703 True Heading 90 00000 180 00000 90 00000 0 00000 Table 1 Magnetometer performance in four different directions As shown from the above table the magnetometer was able to track the heading direction in two of the four directions It was able to track the 90 degree and 180 degree headings fairly 201Page accurately However for the 90 degree and 0 degree headings the magnetometer was completely incorrect The following figure displays the heading tracking of the four directions 85 845 84 1 2 3 4 5 6 7 8 9 10 1555 155 1545 1 2 3 4 5 6 7 8 9 10 Head ng 180 131 5 131 u 1305 1 3 4 5 6 7 8 9 10 Headng 90 110 108 106 oS 2 3 4 5 6 7 8 9 10 Figure 9 Heading tracking of magnetometer The above figure illustrates the overall heading tracking ability of the magnetometer The figure shows that the heading remains fairly constant however it is incorrect in two of the four directions Hence we realized that we cannot use the magnetometer to correct the heading of the robot On the other hand since the heading is correct on two of the four directions we can make use of the magnetometer to initialize the robot s heading More research
57. 0449 070484262704 4 32543611526 2247534218750 0 080063857138 0 67265921831 9 812043190002 0 00681537576 Q 00963919144 0 01852145791 0 26276439428 0 06422121405 0158558571338 155 90592956543 070805960893 4 36984872817 22 47 53 4218750 0 087188281118 0 69446253776 9780597686767 0 05740978196 0 00142790260 40 00720201572 0 26368471980 0 09327381104 0158335763216 155 9130096435 0 70509827136 438365507125 22 47 53 4218750 0 094525553286 0 655393546628 9 798748016357 001391337811 001408906535 000300040468 026994639310 0 05548093095 057909002403 1559909057617 L71914571523 4 38866758346_ 2247534062500 0 087166070938 067976093292 9 784254074096 005493031442 001012907064 0001629662490 026890964969 005059049037 057821625471 1559542296328 071004897356 4 39245557705 2247 53 4062500 0 087258368730 0 68719542026 3791629791253 005650474192 0102756796375 0 014897196553 026459562778 007864390313 050731174468 1559021911621 071274650096 4 39005016234 2247524062500 0 038316386342 0166523794399 9 809712409973 0100281654787 001617261767 0 005798457656 026471811532 0 08352246880 058559636903 155 8918457031 070009664039 4 35604906082 22 47 53 3906250 0 083541892468 0 67613476514 9 789865493774 004021333530 000343472883 001171352434 026485943794 009070044219 058315349605
58. 3 Compute Kalman Gain 3 K k P k H k inv H k P k H k R k where P k is prediction Time Update 3 R Variance of the measurement noise K P H inv H P H R K 3 2K 3 10 6 oo Update the estimate with measurement y k x k x k K k y H k x k x xK K z hx where x k is prediction Time Update innov z hx corr K innov corr 3 0 X x corr oo Compute error covariance for update estimate k I K k H k P k where P k is prediction Time Update eye size K 1 K H P Joseph form better numerical behavior Brown p 261 eye size K 1 K H P eye size K 1 K H K R K oo P P oe oe y 9 show adaption to current state disp corr 82 Appendix J EKF Initialization MATLAB Codes function x P Initialize State 2 sampleMACs sampleRSSIs h numSamples size sampleMACs 1 samplePos zeros numSamples 2 posAvg 0 0 disp numSamples for i 1 numSamples samplePos i Get WiFi Position sampleMACs i sampleRSSIs i end posAvg 1 mean samplePos 1 posAvg 2 mean samplePos 2 posCov 1 15 2 std samplePos 1 2 posCov 2 15 2 std samplePos 2 2 3 numMags length imuMagData 3 sampleHead zeros numMags for i 1 numMags sampleHead i atan2 imuMagData i 1 imuMagData i 2 3 end 3 headAvg mean sampleHead 3 headCov std sampleHead 2 sheadAvg 5 pi sheadCov
59. 5 1560101165771 066777640581 431490230560 2247539531250 01098442889750 064701980352 9 8317699432373 003117465972 000336816604 0 001523935701 026813903450 00606263815 058016359806 1155 9367370605 Q66110990510 430597496032 2247539531250 D 083645259274 072578203678 9 780747413835 00542540243 0004051580538 0 00942266825 1026598717748 007699175622 059565160970 1558941497802 0 67292487621 430505159835 22 47 53 9375000 0091126121580 069107252353 9 813630104064 000347729097 0 005628610029 O01187785621 0 26530534029 007561320066 058646637201 1559019622802 067162209749 28809690475 2247525875000 0079968130395 0 68530142307 9 784454345703 002164781838 0 009799468331 0 00292881904 026515056305 008404950959 056472961197 1559999928984 069447965383 4 2853538605835 2247524375000 0 083660691976 0166891782475 9906419372558 0 03183512389 00283806901 027111265063 0 05538876354 0 58042258024 1559647827148 070022640612 4 30446290969 22 47 53 4375000 0 090799674391 064669996500 9795145998464 0101023893617 0005575199822 002070135623 026951967341 005944342336 057821619510 155 9850170888 069472742080 4 30035924911 2247534375000 0 074356220662 068345397710 9792753944396 0 039915047585 000049261254 0 00987697951 026414701342 00765241699 0 59596529541 155307196
60. 6 14 49 24 613 611 8 4 2 6 09 og 735 3 6 6 6 18 856 6 8 6 6 06 978 4 11 9 6 09 6 15 6 08 mean 125 36 6 268 6 115 Table 6 move 50i test results 60 The first conclusion that can be drawn from the velocity and position measurements above is that as the step size increases the difference between actual measured velocity and computer measured velocity decreases Computer Velocity vs Measured Velocity a Na E 2 o o o 2 Actual Velocity Computer Velocity Figure 34 Computer Velocity vs Measured Velocity After carefully examining the plot above Case 4 where the step size is 25 inches seems to be the best case in terms of the accuracy of velocity Once the step size is increased to 50 inches the actual velocity and the measured velocity starts to diverge from each other The drift can also be compared between these five different cases The next figure shows the initial positions and the final positions of five cases 61 o 2 G 5 Ta o o o x 10 y coordinate Figure 35 Drift comparison for five different cases The figure above shows the initial positions and the final position of the robot for five different cases The blue dot with O indicates the initial position The red square is move 5 i command the green triangle is move 10 1 and the next ones are 15 25 and 50 1 respectively We can conclude that as the step siz
61. 8 4062500 2247 31 0837500 2247 35 8281250 22 47 34 5625000 2247 33 3125000 2247 31 9687500 2247 307031250 2247 28 4531250 22 47 28 1718750 22 47 26 9062500 2247 25 6562500 22 47 24 4062500 22 47 23 0937500 2247 21 8281250 2247 20 5625000 1224718312500 2247 17 9687500 224716 7187500 22 47 15 3281250 243750 1952887342534 4823 832225455 1846 523261158 4790 498219995 1913541738051 4751 1094820233 1913541738051 4751 1094820233 1913541738051 4751 1094820233 1913541738051 4751 1094820233 1813541739051 4751 1094820233 1913541738081 4751 1094820233 1913541738051 4751 1094820233 1913541738051 4751 1094820233 1905 758667961 4766 037858681 2043 202464146 4822 956838202 2041 501441173 4827 483275765 2054216334517 4788 412734026 2054 216334517 4788 412734026 2054 21633 ge Figure 20 Robot location server interface showing the different sensors The IMU is another sensor implemented in the ERI Locator Server using direct communication with the USB COM Device The IMU is updated at a frequency of 100Hz and at every specified update interval currently 1 second the ER1 Locator Server computes the total angle displacement over that interval and runs a MATLAB script to incorporate this angular displacement into the localization algorithm to update the current calculated position Not only are the cu
62. 849 22 47 47 5289165 22 50 19 6437497 22 51 01 4918043 22 52 26 1509376 22 53 07 8209877 22 53 36 9397215 22 54 33 7451529 22 55 03 3568991 22 55 05 3339490 22 56 16 3787392 22 56 21 0728575 23 05 11 9782360 Figure 22 Ground truth application for logging the actual position of the robot with time As shown from the above figure by using the ground truth application we can easily keep track of the real position of the robot as well as its corresponding time With the measurements we can easily pass it through our ground truth plotting script to build the ground truth plot Our script can be found at Appendix L Finally with the ground truth application and script we can test the results of our three algorithms The first section is the dead reckoning algorithm which combines the odometer from the robot with the gyroscope from the IMU 43 Page 5 2 Performance of the Dead Reckoning Algorithm For combining the odometer of the robot and the gyroscope of the IMU data we implemented a very basic algorithm that tracks the change in distance and change in angle By using the angular velocity reading in conjunction with the linear velocity reading we can compute the new position and angle of the robot given that we already know the previous position and angle This algorithm primarily predicts your new position given that you
63. 9 throw new ArgumentException covariance must be length 3 3 or 5 5 cowvariance this state new double state Length this covariance new double state Length state Length Extract state values for int i 0 ix state Length i this state i double state GetValue i for int j 0 j lt state Length j this covariance i j double covariance GetValue i label not set Gets Sets current EKF State value x y heading velocity angular velocity public double State get return state if value Length state Length amp amp value Length 5 amp amp value Length 3 return state value public double Covariance get return covariance 125 if value Length 25 amp amp value Length 9 return covariance value public double State3 get double st new double 3 for ing 1 0 Xi 3 itt st i State i return st public double Covariance3 GEE f double cov new double 3 3 for int i 0 1x 3 itt ior int J 07 gy 32 JTF cov i j Covariance i j return cov public double XPos get return state 0 set state 0 value public double YPos get return state 1 set state 1 value public double Heading get return state 2 set state 2 value public double HeadingDegrees
64. AN RENTES SA EAE E EEEE ERES a UR iat 31 3 22 mie Update GA TA 32 3 2 3 Measurement Update Stage iicsdiectee Us bvd t e edd c hse Rad e NS IN P GG 33 CHAPTER IV SOFTWARE DESIGN AND INTEGRATION eere 35 4 T Overview of Project ATCC CHUTE isone a e ve a bade a id c A a a Real 35 42 JAVA AA AA 36 4 3 Robot Location Sensor SEEK a Seat fe toc titus An a pant ae Nod BA aaa 38 CHAPTER V PERFORMANCE ANALYSIS s eeeeeeeeeeeeeeeen nennen nennen enne tnn 42 5 1 Generating Ground Truth for Reference esses eee enne enhn enne 42 5 2 Performance of the Dead Reckoning Algorithm eese 44 5 3 Performance of Wi Fi Localization Algorithm 47 5 4 Performance using the Extended Kalman Filter for Fusion 51 5 5 Comparative Performance Analysis nre e Ee OR EA REUS Intr Pat eh nte dn taa deg 54 CHAPTER VI CONCLUSION e iteedectsct ee pia AKA riter maa NA uh b eode i raid 56 Future V ford D kaaga 57 AcknbwIedvemelte AAH as SA 58 APPENDICES manna maa pa ANA 59 Appendix A ER 1 Position amp Velocity Measurements 10 15 25 50 in 59 Appendix B Wi Fi Localization Flow Chart esee nennen 63 Appendix C sonar c Code that is developed for Vex Ultrasonic Rangefinder 64 Appendix D Laser Optical Line Tracker Sensors eene 66 Appendix B Kalman PHIer s el deter or IR Mic
65. Callback d new DataGridAddRowCallback SafeDataGridAddRow this Invoke d new object dataGridView data else dataGridView Rows Insert 0 data else dataGridView Rows Add sensorData ToArray private void btnStart Click object sender EventArgs e Button btnStart Button sender if btnStart Text Start Start remote communicator and update button if successfull if rc Start btnStart Text Stop else if btnStart Text Stop rc Stop btnStart Text Start private void btnERlConnect Click object sender EventArgs e if btnER1Connect Text Connect string ipAddress txtERlIPAddress Text int port int Parse txtERlPort Text erlController Connect ipAddress port 184 if erlController connected btnER1iConnect Text Disconnect else MessageBox Show Cannot disconnect from ER1 btnER1Connect Text Connect private void btnstartUpdatingER1 Click object sender EventArgs e if erlController isUpdating Stop updating if already updating erlController stopUpdating else We are not already updating so startUpdating Start getting ER1Position data every TimeInterval erlController startUpdating private void btnStartUpdatingIMU Click object sender EventArgs e if imuSensor isUpdating Already update so stopUpdating
66. Controllers erlController imuController erlController wifiSensor positionAlgo new MLApp MLAppClass Console WriteLine Cannot Start camera connect if camera started Console WriteLine COULD NOT INIT CAMERA Console WriteLine Trying to close and reconnect if camera disconnect if camera connect Console WriteLine STILL COULD NOT INIT CAMERA ThreadPool SetMaxThreads 1 1 ThreadPool SetMinThreads 1 1 Create the HTTP Listener to accept requests from the network listener new HttpListener listener Prefixes Add http 127 0 0 1 listen on local loopback listener Prefixes Add http localhost listen on localhost listener Prefixes Add http listen on localhost IPAddress addresses Dns GetHostAddresses Dns GetHostName listen on ip address for int i 0 i lt addresses Length i string address addresses i ToString 172 if address Contains continue Console WriteLine Listening on address address listener Prefixes Add http address baseFolder send command private void HandleRequests while true try HttpListenerContext request listener GetContext ThreadPool QueueUserWorkItem ProcessRequest request ThreadPool UnsafeQueueUserWorkItem ProcessRequest request catch HttpListenerException break catch InvalidOperationException br
67. E RTTE E A KAN GINA NA TAGA ai 54 Figure 33 Cumulative density function of the three algorithms extended Kalman filter wi fi localization amd dead TOC ROHIDE a E NAA AA EO E EU DU UE 55 Figure 34 Computer Velocity vs Measured Velocity esessseessesssesssresseseresressesererresserererreeseese 61 Figure 35 Drift comparison for five different cases sssessesseseeeeenen nenne Figure 20 Wiser PIG we Bart cs eod aesti tette eu dtum AA raihi ias 63 Figure 37 7 VexLaue Tracker SENSON gest dade quede AA anas ior teal eden aaa 69 Figure 38 Laser Readings and Ultrasonic Sensor Readings seeee 71 Figure 39 Laser vs Ultrasonic Sensor Readings eie cet Eee tei t asawa tcr naaa 72 Figure 40 Vex Ultrasound Sensor mounted to four different sides on the robot 73 Figure 4l Kalman Filter Model to oa peer tondere idee re deis 75 Fig re 42 Kalman Filter RESUlts 2 urn Seta ha dotted Dane tite taotum utut idee 77 Figure 43 Raw Wi Fi Red vs KF Wi Fi Black Results 78 6lPage ABSTRACT The purpose of our major qualifying project is to develop a localization method for precise indoor geolocation There are two phases in our project The first involves devising an algorithm to combine the readings from four different sensors a gyroscope odometer ultrasound sensors and Wi Fi Received Signal Strength readings Under this phase we designed and implemented a fusion algorithm to incorp
68. Hybrid Indoor Geolocation for Robotic Applications A Major Qualifying Project Report Submitted to the faculty of Worcester Polytechnic Institute In partial fulfillment of the requirements for the Degree of Bachelor of Science By Yuan Shi Billy Zhong Ramsey Abouzahra Cengiz Karakoyunlu On 29 April 2010 1IPage Table of Contents ABSTRACT a T NG 7 CHAPTER I INTRODUCTION NAAN AA AGA 8 T BAN AA AA APO aded 9 I2 PEE V TOUS WORK aan ana pa AN a aNG 10 1 3 Project DESETIPUON ab GAGA RR TUI ERA IRE s KAG KAG KA ikasa 12 NA Project Outlines aaah ain GIA AE 14 CHAPTER II Analysis of Sensor Behaviors esee ener enne enne 15 2 1 Sensing the RSS OP MI AA AAP 15 2 2 Inertial Measurement Unit for Angle Tracking esee L7 2 2 1 Gyroscope for Sensing Angular Velocity essere 18 2 2 3 Magnetometer for Sensing Initial Heading eene 20 2 3 Odometer for sensing veloctty oes end ee v te ea NE INA REN RR NE Se YA NAR US SITE UR Pea UND GE na UN Pe PRA 22 2 4 Ultrasound Sensors for Correcting Heading esses 23 CHAPTER III LOCATION AND TRACKING ALGORITHMS eee 2 3 1 Kernel Method for Wi Fi Localization 4A AG ANNA LG kA GAGA 28 3 2 Extended Kalman Filter for Sensor Fusion eee eerte ee een ene een n set en neos en ase 30 3 2 1 Initialization Process i i ect ina ENTE EAR BU
69. LECT FROM MSNdis 80211 ReceivedSignalStrength Where active true ObjectQuery query new ObjectQuery SELECT FROM MSNdis 80211 BSSIList WHERE active true ManagementClass mc new ManagementClass root WMI ManagementScope scope new ManagementScope root wmi ManagementObjectSearcher searcher new ManagementObjectSearcher scope query string result foreach ManagementObject obj in searcher Get if bool obj Active true Console WriteLine obj InstanceName foreach PropertyData prop in obj Properties result prop Name obj prop Name Environment NewLine ManagementBaseObject bssilist ManagementBaseObject obj Ndis80211BSSIList for int i 0 i lt bssilist Length i System Byte MACAddress System Byte bssilist i Ndis80211MacAddress result Nt MAC BitConverter ToString MACAddress ToArray lt byte gt 188 int RSSI OFFSET UInt32 RSSI UInt32 bssilist i Ndis80211RSSI double dBm 10 Math Log10 1000 0 RSSI RSSI OFFSET 121 result Nt RSSI dBm Convert ToString UInt32 bssilist i Ndis80211RSSI result Environment NewLine Console WriteLine bssilist i Ndis80211RSSI GetType foreach PropertyData prop in bssilist i Properties if result t prop Name bssilist i prop Nam
70. Low Cost Optical Indoor Localization System for Mobile Objects without Image Processing Rep Web lt http www imd uni rostock de veroeff psd salomon 06 pdf gt Chitra Pushkaraj Pradeep and Kaveh Pahlavan Localization Algorithms and Dynamic Channel Behavior for Urban Geo location Rep Print Kiriy Eygeni and Martin Buehler Three State Extended Kalman Filter for Mobile Robot Localization Tech 12 Apr 2002 Web http www cim mcgill ca kiriy publications eKf 3state pdf gt Welch Greg and Gary Bishop An Introduction to the Kalman Filter Publication 24 July 2006 Web http citeseerx ist psu edu viewdoc download doi 10 1 1 79 6578 amp re p rep1 amp type pdf gt 194 12 13 14 15 16 P Bahl and V N Padmanabhan RADAR An in Building RF based user location and tracking system Proc Of IEEE INFOCOM 2000 vol 2 pp 775 784 March 2000 lt http research microsoft com en us people padmanab infocom2000 pdf gt Xsens Technologies Magnetic Field Mapper MT Manager Add on User Manual 2008 Print Xsens Technologies MTi and MTx User Manual and Technical Documentation 2008 Print Xsens Technologies MT Manager User Manual Rep 2008 Print Evolution Robotics ER User Manual Print 195
71. Magnetometer Extended Kalman Filter Post Processing of Data Displacement Kernel Method Computer Sensor Data Acquisition Location Sensors Figure 3 Overview of the system architecture illustrating the sensor integration as well as the remote control video stream map display and two way communications Not only can the GUI remotely control the robot as shown above it can also log data in real time such that a performance evaluation can be performed This yields to a far better and 131Page more accurate analysis of the entire algorithm Lastly these evaluations also reveal the dissimilarities between implementing an EKF in real time and simulating one based on data 1 4 Project Outline Chapter I is the introduction which gives the overview of our project Hybrid Indoor Geolocation for Robotic Applications It discusses the motivation and contribution of our project Chapter II provides an analysis of the behaviors of the sensors used in our project such as Wi Fi IMU odometer from the ER 1 Robot and VEX Ultrasound sensor Chapter III explains the algorithms used to integrate the multiple sensors For example we used the Kernel method to predict the position of the object based on RSS readings from Wi Fi It also describes how the Extended Kalman Filter is implemented to combine the readings from all the sensors Chapter IV depicts the graphical user interface GUI that is designed to implement Extended Kalman Filter in
72. PI class ER1Controller SensorController Requests to be handles by IMU Controller private static string validRequests move stop position events set okay ER1 Robot uses Telnet for remote control and data acquisition private TelnetConnection telnet private string hostname 141 private int port private Button btnStartUpdating private int lastCount Setup ER1 Controller to connect to the ER1 Remote API public FRIController string hostname int port Button btnStartUpdating base this hostname hostname this port port telnet new TelnetConnection hostname port this btnStartUpdating btnStartUpdating Setup ER1 Controller to poll at the specified interval public ER1Controller string hostname int port int interval Button btnStartUpdating base interval this hostname hostname this port port telnet new TelnetConnection hostname port this btnStartUpdating btnStartUpdating lastCount 0 Connect to the ER1 Remote API Telnet public bool Connect string hostname int port if telnet null telnet new TelnetConnection hostname port return telnet IsConnected return telnet Connect hostname port public override void startUpdating If not connected connect to ER1 Telnet Remote API if connected this Connect this hostname this port
73. Pixels height 2 width height g fill3DRect xPixels width 2 yPixels height 2 width height Draw robots path g setColor Color RED for int i 0 i lt robot getTrace getPoints size i Point pastLoc robot getTrace getPoints get i g draw3DRect CMToPixels pastLoc getX CMToPixels pastLoc getY CMToPixels robot getWidth 2 CMToPixels robot getHeight 2 true CMToPixels pastLoc getY CMToPixels robot getWidth 2 g fill3DRect CMToPixels pastLoc getX CMToPixels robot getHeight 2 true cobotTrace render g 122 Appendix P 1 Robot Server Models Appendix P 1 1 SensorData cs using System using System Collections Generic using System Ling using System Text using System Windows Forms namespace ERlLocationListener Models class SensorData private DateTime timestamp public SensorData Use current time as timestamp timestamp DateTime Now public SensorData DateTime time timestamp time public DateTime Timestamp get return timestamp set timestamp value public override string ToString return Timestamp timestamp n virtual public object ToArray return new object timestamp public static bool isValidSensorData SensorData sd return sd null amp amp sd Equals NoSensorData
74. Pos value public double Angle get return angle set angle value public double AngleDegrees get return angle 180 0 Math PI set angle value Math PI 180 0 public double Velocity get return velocity set velocity value public override string ToString string str xPos xPos An str t5 yPos yPos n str angle angle An return str base ToString public string ToResponseString return OK XPos ToString YPos ToString AngleDegrees ToString NO Converts ER1Postion to array of objects for DataGridView override public object ToArray return new object xPos yPos AngleDegrees velocity Timestamp TimeOfDay Converts ER1Postion to array of objects for Matlab public double ToMatlabArray 129 return new double xPos yPos AngleDegrees velocity Timestamp TimeOfDay TotalMilliseconds command Parse the response from the ER1 Telnet Remote API s position public static ER1Position Parse string posStr LU Ww posStr posStr Trim if posStr null posStr Equals NOT CONNECTED posStr throw new System ArgumentException Invalid ER1 Position String posStr pu pu pu measuremen pu position t oo Dn pub measurement
75. Sensor e Range from 20 mm to 500 mm e Accuracy within 10 mm e Computer interface RS 232 or RS485 e Price 631 Table 9 Leuze ODSL8 Optical Distance Sensor Leuze ODS 96 Series Optical Distance Sensor e Range from 60 mm to 5000 mm e Accuracy within 10 mm e Computer interface RS 232 or RS485 e Price 689 Table 10 Leuze ODS 96 Series Optical Distance Sensor The literature search about optical distance sensors prove that using this type of sensors would be more appropriate in terms of staying in the limits of budget Optical sensors have lower range than laser sensors yet their accuracy is also lower than the accuracy of laser sensors The advantage of optical sensors over laser sensors is their lower price As a result ultrasonic sensors 67 will be more feasible for our study than other sensor types The ultrasonic sensors that are found after literature search and their features are shown below Maxbotix XL MaxSonar EZ4 High Performance Sonar Module e Range from 15 cm to 645 cm e Accuracy within 3 cm e Computer interface None requires analog circuitry e Price 25 Table 11 Maxbotix XL MaxSonar EZ4 High Performance Sonar Module Vex Ultrasonic Kit e Range from 3 cm to 3 m e Accuracy within 1 cm e Computer interface USB e Price 30 Table 12 Vex Ultrasonic Kit The literature
76. Sensor CurrentData if curIMUData Equals Models SensorData NoSensorData curPosWiFi Angle Math Atan2 curIMUData MagYVal curIMUData MagXVal return curPosWiFi ToResponseString 176 else if request Equals getEKFPosition try f Models EKFState currentState Models EKFState EKFAlgorithmClass CurrentState return currentState ToResponseString catch Exception ex f Console WriteLine No EKF State return new Models FRIPosition 0 0 ToResponseString else if request Equals customPosition CUSTOM Algorithm for position if positionAlgo isUpdating erlController connected eriController isUpdating imuSensor isUpdating return NOT UPDATING ALL SENSORS positionAlgo updatePosition Updating at TimeInterval now return positionAlgo ToString Gives out latest position calculated from algo else if request Equals resetCustomPosition positionAlgo resetPosition return positionAlgo ToString else if ER1Controller handlesRequest request Handle Command requests mov position etc R1 tj if lerlController connected return Not Connected to ERI m Telnet API return erlController sendCommand request else if request getPosition Simulation built into erlController for no connectionz return erlController CurrentData T
77. XMLDecoder fis Read object Map readMap Map xdec readObject this width readMap width this height readMap height this lines readMap lines return true Appendix 0 1 3 ER1RemoteConnection java package model import java i import java n public class S Qus et URL ER1RemoteConnection private String address public ERIRemoteConnection String address this address address public String sendCommand String command command command replace 20 String url http taddresst send command tcommand return httpRequest url private String httpRequest String requestURL String response try URL url new URL requestURL BufferedReader in new BufferedReader new InputStreamReader url openStream String inputLine 94 while inputLine in readLine null response inputLine in close catch IOException e e printStackTrace if response return NO RESPONSE return response Appendix 0 2 Robot Remote Control Client ER1Navigator Controllers Appendix 0 2 1 ER1NavigatorFrame java import javax swing import java awt import java util Hashtable n import java util Timer n import java util TimerTask import models import views import controllers public class ER1NavigatorFrame extends JFrame private JSlider
78. YPE NEAREST NEIGHBOR mapImage op filter mapImage null e zoomMapImage mapimage Handle Zoom Slider change and Detail View Checkbox public void stateChanged ChangeEvent e if zoom slider changed if e getSource equals zoomSlider if zoomSlider getValueIsAdjusting double zoom zoomSlider getValue 100 0 118 if det ailCheckBox isSelected zoomMapImage mapimage this repaint double mapPixelsPerCM 20 0 180 0 20 ed pixels for every 75 cm unscal maplImage getWidth maplImage getHeight robotView setZoom mapPixelsPerCM 25 zoom is no scaling of image zoomWidth int 4 zoom zoomHeight int 4 zoom zoomMapImage maplImage getScaledInstance zoomWidth zoomHeight Image SCALE DEFAULT for every 75 cm unscaled else if the detailed this repaint double mapPixelsPerCM 20 0 75 0 20 pixels robotView setZoom 4 zoom mapPixelsPerCM mapView setZoom zoom robotView setZoom zoom else if e getSou this repaint public JSlider getZoomS1l return zoomSlider public JCheckBox getDeta return detailCheck public JLabel getHeading return headingLabe Display the Map and t public void paintCompone Paint backgroun super paintCompone Display the Map if detailCheckBox disabled mapView pain series of lin
79. a pedo eee Ka 148 Appendix P234 WIFICOntroll er S ia er ree c eus 152 Appendix P2 SenarController6s 5i nah aa abakada 157 Appendix P 2 05 WIEMDB OS s eese dtt est t Dc pond og E cestode mita 161 Appendix P 2 7 EKFAlgorithmClass e8 eiecti eed leet eei Kana 163 Appendix P 2 8 RemoteCommiiloator 8 osea ede tt pe resi esas GAGA 171 Appendix D 2 9 Ponies i2 oe A BEE G 179 BIBLIOGRAPHY onnon paaa NG PAA AJA AA 194 4 Page Table of Figures Figure 1 Indoor robotic applications requiring location awareness eene 9 Figure 2 General concept of the multi sensor location aware robot with real time remote control and TIA CAS DIA e adtee us eto dion coorta isctahadie AA 12 Figure 3 Overview of the system architecture illustrating the sensor integration as well as the remote control video stream map display and two way communications sssss 13 Figure 4 Database for applying the Kernel method algorithm for wi fi position estimate 16 Figure 5 Overview of the inertial measurement unit and its tracking coordinates 17 Figure 6 Inertial measurement unit data log format eui ratae rine on Ite edd 18 Figure 7 Gyroscope readings of right turn made around the third floor of Atwater Kent nciht R E 19 Figure 8 Gyroscope readings of left turn made around the third floor of Atwater Kent 19 Figure 9 Heading tracking Of
80. aing the status of if request Equals status response ER1 Remote Access runn response erlController ToString response imuSensor ToString response nTimestamp return response if request Equals wifiscan Read wifiscan data from POST web Stream readStream context Reques DateTime Now LAG sans NONA 7 NRY Aq B pts request from inSSIDer t InputStream byte data new byte context Req readStream Read data 0 data Leng Convert data to string and save ASCIIEncoding encoding new ASCII string wifiScanData encoding Get Save current wifidata lock wifiSensor uest ContentLength64 th for later use Encoding String data wifiSensor ReceiveWiFiData Models WiFiData Parse2 wifiScanData Console WriteLine WiFi found wifiSensor CurrentWiFiData Length Access Point gute lse if request Equals wifiscan_ result return the last scanned wifidata return wifiSensor CurrentWiFiData ToString ER1 Remote API is Connected request lse if request Equals isERlConnected if erlController connected 175 return CONNECTED else return NOT CONNECTED Handle IMU Rquests else if IMUController handlesRequest request Return the last received IMU Data Accel Gyr Mag and
81. ameButton btnStart Start Updating WiFi Read the current values from the WiFi and store it override public void doUpdate object sender System Timers ElapsedEventArgs e Called every time interval Wait for this Update to Complete before starting next Update timer Stop DateTime start DateTime Now SensorData newData SensorData NoSensorData GEY Get the current wifi access points Array MACs CurrentWiFiData MACArray Array RSSIs CurrentWiFiData RSSIArray Array dummyl new string MACs Length Array dummy2 new double RSSIs Length if MACs Length 10 return lock matlab f Execute the Matlab script to read values matlab Execute cd scriptPath Change Directory to Matlab Script Folder Put MAC RSSI List for WiFi Get Position into MATLAB workspace matlab PutWorkspaceData MACs base MACs matlab PutFullMatrix RSSIs base RSSIs dummy2 string response matlab Execute position Get WiFi Position MACs RSSIs i Console WriteLine WiFiController script output response DateTim nd DateTime Now Console WriteLine WiFi Update to execute took end TimeOfDay Subtract start TimeOfDay n Get the updated values from Matlab Array position new double 2 Array dummy new double 2 dummy variable for matlabs imaginary matlab GetFullMatrix position base ref position ref dummy 154
82. an clearly see that the Extended Kalman Filter performed the best The plot shows that 90 of the error in the EKF is within 2 7 meters For Wi Fi Localization 90 of the error is within 4 5 meters The problem in Wi Fi is that the algorithm is shows too much instantaneous variations in position estimates It cannot be used for tracking continuous movement Lastly in the Dead Reckoning Algorithm almost 75 of the error is greater than 5 meters The weakness in the Dead Reckoning Algorithm is that once angle becomes off the entire algorithm fails However it does a good job in tracking continuous movement With the EKF it combines the continuous tracking ability of Dead Reckoning with the accuracy of Wi Fi to output the most accurate position estimate 55 Page CHAPTER VI CONCLUSION In conclusion we designed implemented and tested a location aware multi sensor robotic platform for indoor applications taking advantage of an existing Wi Fi infrastructure Our platform has three main features The first feature is a user friendly graphical user interface with the flexibility to log data using different fusion algorithms for evaluation and post processing One of the purposes of the software architecture is that it allows for the application of location aware robots The interface contains a real time map display remote control and data logging capabilities With the data log feature we can easily test and evaluate the performances of differen
83. asFile f int sampleNum 1 matlab Execute sampleMACs cell WE matlab Execute sampleRSSIs findMaxAPs matlab PutWorkspaceData MACs base wifiData MACArray matlab PutWorkspaceData RSSIs base wifiData RSSIArray matlab Execute sampleMACs sampleNum 1 length MACs MACs matlab Execute sampleRSSIs sampleNum l length RSSis RSSIS sampleNumtt if asFile matlab Execute save name MACs mat sampleMACs matlab Execute save name RSSIs mat sampleRSSis Appendix P 2 7 EKFAlgorithmClass cs using System using System Collections Generic using System Ling using System Text using ERlLocationListener Models using System Collections using System Windows Forms namespace ER1LocationListener Controllers static class run scripts Script to samples Count zeros samples Count foreach WiFiData wifiData in samples EKFAlgorithmClass ArrayList ReceivedData EKFState currentState MLApp MLAppClass matlab tatic Latip Latis private s private s private s Latic string to Read Values from IMU private s Execute scriptName Time Update Create MATLAB object to Matlab SensorData data public delegate void SensorUpdateReceievedDelegat private static SensorUpdateReceievedDelegat didUpdate
84. ate transition matrix is the Jacobian of the state or also known as the derivative of the state which is shown in great detail above Note this matrix is only used during the covariance update phase Now that all the elements in the prediction state are explained let us move on to the measurement update phase 3 2 3 Measurement Update Stage Similar to the Kalman Filter KF the EKF also implements a set of mathematical equations to update the Kalman gain state matrix and the covariance 7 HT yT 1 amp amp Ky zi h 27 0 P I K H P Eq 6 This part of the algorithm is very similar to the KF in the sense that it uses almost the exact same elements to derive the Kalman gain state matrix and covariance Typically the measurement function only sets up the subtraction terms as stated in 10 In the Measurement Update part of the EKF we used the following equations which are shown in Appendix I bl Ba Zk k y q The first part of the measurement update phase is to determine what is actually being measured In our case we chose to measure the Wi Fi x and y coordinates and we will only be using these values to update the EKF position only when the covariance values become too large 7 Kiriy and Buehler Page 5 33 Page dh Oh OX k Qyk dh 9h OX Qyk Xk c a Eq 8 c During the measurement update phase the raw Wi Fi readings will be directly applied to the current state onl
85. bot private LoggerController logger private JLabel lblLogFile private JTextField txtLogFile private JTextArea txtLogArea private JScrollPane scrollLogArea private JButton btnStartLogging private JButton btnReplay private JButton btnClearLog private JButton btnBrowse private JFileChooser fcLogFile private JTabbedPane logTabPane public LoggerPanel Robot robot LoggerController logger f this robot robot this logger logger initializeComponents logger setLogArea txtLogArea setVisible true public void initializeComponents this setLayout new GridLayout 2 3 Create log Control Panel 114 JPanel logControlPanel new JPanel lblLogFile new JLabel Log File txtLogFile new JTextField 32 fcLogFile new JFileChooser btnBrowse new JButton Browse btnBrowse addActionListener new ActionListener public void actionPerformed ActionEvent e select file logControlPanel add lblLogFile logControlPanel add txtLogFile logControlPanel add btnBrowse Add Start Logging Button btnStartLogging new JButton Start Logging btnStartLogging addActionListener new ActionListener public void actionPerformed ActionEvent e start logging logControlPanel add btnStartLogging Add Replay Button btnReplay new JButton Replay btnReplay addActionListener new ActionListener
86. c static void Clear ReceivedData Clear public static MLApp MLAppClass Matlab get return matlab set matlab value public static string ScriptName get return scriptName set scriptName value static public string ToString return currentState ToResponseString Appendix P 2 8 RemoteCommunicator cs using using using using using using using Sys Sys Sys Sys Sys Sys Sys ERI cem cem cem cem jOC cem cem cem Collections Generic Ling Text Net Threading BOT ationListener Controllers using namespace ERI1LocationListener class RemoteCommunicator HttpListener listener 171 lder Thread string baseFol Thread worker Location Sensing Hardware Controllers ER1Controller erlController IMUController imuSensor WiFiController wifiSensor CustomAlgorithm positionAlgo MLApp MLAppClass matlab int fails Models WiFiData wifi public RemoteCommunicator ref IMUController imuController CustomAlgorithm positionAlgo fails 0 this Lhis this this imuSensor wifiSensor positionAlgo Create Matlab bry matlab catch Mat Labw 2 lt bool camera_started Data ref WiFiController wifiSensor Exception ex null ER1Controller erlController ref ref Location Sensing Hardware
87. can be used in factories and science lab to provide firsthand experience for those who are not able to travel With the aid of location information many indoor robotics services can be made possible Acknowledgements The authors would like to express their thanks to Professor Kaveh Pahlavan and his PhD students Ferit Ozan Akgul and Yunxing Ye for their support assistance and inspiration during the implementation period of this work 58 lPage APPENDICES Appendix A ER 1 Position 4 Velocity Measurements 10 15 25 50 in t 4 s instructed 10 inches 25 4 cm Case2 cwins lab comp velocity cm s 5 72 5 75 5 75 5 72 5 78 5 8 5 8 5 78 230 8 0 4 5 78 5 771 Table 3 move 10i test results Case3 cwins lab t 6s instructed 15 inches 38 1 cm comp measured cm velocity cm s aa azoa 5 77 6 05 mean 37 918 6 32 6 059 Table 4 move 15i test results Case4 cwins lab t 10s instructed 25 inches 63 5 cm measured comp measured cm velocity cm s velocity cm s 6 11 1224 02 6 13 1 183 6 0 6 612 612 3060 21 612 6 1 6 12 61 612 mean 61 12 6 115 6 117 Table 5 move 25i test results Case5 cwinslab t 20s instructed 50 inches 127 cm comp velocity cm s 6 12 223 00 EMEN NENNEN A 2446 04 6 1 367 4 1 3
88. ception ex Console WriteLine WiFi Update Received but failed updating Message ate void SonarUpdateReceived Models SensorData sonarData Check Sensor Updating was Stopped if sonarData Equals Models SensorData SensorUpdatingStopped SafeRenameButton btnStartUpdatingIMU Stop Updating WiFi Adds Sensor Data to Sensor Data Grid View SafeDataGridAddRow dgvSonarData sonarData andles receiving new positions from custom algorithm ate void PositionUpdateRecieved Models SensorData position Adds ER1Postion to Positioning Algorithm Data Grid View SafeDataGridAddRow dgvAlgorithm position Adds ER1Postion to ER1 Data Grid View SafeDataGridAddRow dgvAlgorithm position if dgvAlgorithm InvokeRequired f 182 DataGridAddObjectRowCallback d new DataGridAddObjectRowCallback SafeDataGridAddObjectRow this Invoke d new object dgvAlgorithm position ToArray else dgvAlgorithm Rows Insert 0 position ToArray x Handles receiving new positions from custom algorithm private void EKFUpdateRecieved Models SensorData ekfState Adds ER1Postion to Positioning Algorithm Data Grid View SafeDataGridAddRow dgvEKF ekfState Safely Renames Button public void SafeRenameButton Button button string text if button InvokeRequired RenameButtonCallback d new RenameButtonCallback SafeRenameButton this Invoke
89. curacy range computer interface etc The results of a detailed literature search about sensors point out what type of sensors are available Laser sensors Optical sensors Ultrasonic sensors The laser sensors are the most accurate ones among the sensor types above The laser sensors that are found after literature search and their features are shown below Hokuyo URG 04LX UG01 Scanning Laser Rangefinder e Range from 20 mm to 5600 mm e Accuracy within 30 mm e Computer interface Includes a USB Cable e Price 1284 Table 7 Hokuyo URG 04LX UG01 Scanning Laser Rangefinder Moduloc LT2000 SP Laser Distance Measurement Meter e Range from 200 mm to 30m e Accuracy within 3 mm e Computer interface RS 232 or RS422 485 e Price 980 Table 8 Moduloc LT2000 SP Laser Distance Measurement Meter 66 Laser distance sensors are very accurate as seen in their features above The range they can take measurements is also very long The major problem with laser distance sensors are their price Since we have 400 budget to accomplish this Major Qualifying Project it would not be meaningful to spend the entire budget on one sensor Therefore other sensor types such as infrared and ultrasonic sensors are given priority in literature search The optical sensors that are found after literature search and their features are shown below Leuze ODSL8 Optical Distance
90. d new object button text else button Text text Safely Set Text public void SafeSetText TextBox txtBox string text if txtBox InvokeRequired SafeSetTextCallback d new SafeSetTextCallback SafeSetText this Invoke d new object txtBox text else txtBox Text text Safely Adds SensorData to the List of Data private void SafeListAddData ListBox listData Models SensorData sensorData if sensorData null sensorData Models SensorData NoSensorData if listData InvokeRequired ListAddDataCallback d new ListAddDataCallback SafeListAddData this Invoke d new object listData sensorData else listData Items Add sensorData Safely Adds SensorData to the Data Grid View 183 private void SafeDataGridAddRow DataGridView dataGridView Models SensorData sensorData if sensorData null sensorData Models SensorData NoSensorData if dataGridView InvokeRequired DataGridAddRowCallback d new DataGridAddRowCallback SafeDataGridAddRow this Invoke d new object dataGridView Models SensorData sensorData else dataGridView Rows Insert 0 sensorData ToArray else dataGridView Rows Add sensorData ToArray private void SafeDataGridAddObjectRow DataGridView dataGridView object data if data null return if dataGridView InvokeRequired DataGridAddRow
91. d lower boundaries upperdist upperbound abs curr heading lowerdist abs curr heading lowerbound 3 Decides whether to clip or not as well as which value to clip it to if upperdist lt lowerdist if abs curr heading lt upperbound trange amp amp abs curr heading gt upperbound range temp upperbound else temp end else if abs curr heading lt lowerbound trange amp amp abs curr heading gt lowerbound range temp lowerbound else temp end end If current heading is a negative value 3 then it restores that to the new heading if curr heading x 0 oe oe oe oo curr heading curr heading oo if temp lt 0 new heading temp else new heading temp 1 end else new heading temp end end 84 Appendix L Ground Truth MATLAB Codes ground data xlsread ground truth data4 11 2 num points length xSaved xX pos xSaved 1 num points l1 y pos xSaved 1 num points 2 heading xSaved 1 num points 3 temp time char state info 1 num points 2 ekf times temp time 11 18 ekf times scale zeros num points 1 Convert time to seconds for i 1 length x pos hour to sec str2num ekf times i 1 10 str2num ekf times i 2 60 60 min to sec str2num ekf times i 4 10 str2num ekf times i 5 60 sec str2num ekf times i 7 10 str2num ekf times i 8 ekf times scale i 1 hour to sectmin to sectsec end
92. dStatus public ConnectionPanel Robot robot f this robot robot initializeComponents setVisible true public void initializeComponents 1b18tatus new JLabel Status txtStatus new JTextField 10 txtStatus setText Not Connected this setLayout new GridLayout 4 2 add lblStatus add txtStatus lblAddress new JLabel Address txtAddress new JTextField 10 txtAddress setText 127 0 0 1 add lblAddress add txtAddress lblPort new JLabel Port txtPort new JTextField 10 txtPort setText 9000 add l1blPort add txtPort btnConnect new JButton Connect btnConnect addActionListener new ActionListener public void actionPerformed ActionEvent e connect add btnConnect txtExtendedStatus new JTextArea public void connect Connect to user defined IP Address if robot connect txtAddress getText update status txtStatus setText Connected 111 btnConnect setText Disconnect else txtStatus setText Connection Failed btnConnect setText Connect return txtExtendedStatus setText robot getClient sendCommand test Appendix 0 3 4 ControlPanel java package views import javax swing import java awt event import java awt import java util Hashtable import models Robot import controllers MoveController public class Co
93. e if btnStartUpdatingAl1l Text Equals Start Updating Al1 erlController startUpdating imuSensor startUpdating positionAlgo startUpdating SafeRenameButton btnStartUpdatingAll Stop Updating All else erlController stopUpdating imuSensor stopUpdating positionAlgo stopUpdating SafeRenameButton btnStartUpdatingAll Start Updating All nG otnStartUpdatingER1 Click null null btnStartUpdatingIMU Click null null nG private void btnStartUpdatingWiFi Click object sender EventArgs e if wifiSensor isUpdating Already update so stopUpdating wifiSensor stopUpdating else Not Updating so startUpdating wifiSensor startUpdating private void btnStartUpdatingSonar Click object sender EventArgs e if sonarSensor isUpdating Already update so stopUpdating sonarSensor stopUpdating 186 else Not Updating so startUpdating sonarSensor startUpdating private void btnSaveToMatlab Click object sender EventArgs e positionAlgo ExportToMatlab private void btnWZzC Click object sender EventArgs e f if btnWZC Text Equals Start WZC System Diagnostics Process p System Diagnostics Process Start net start wzcsvc p WaitForExit btnWZC Text Stop WZC else System Diagnostics Process p System Diagnostics Process Start net stop wzcsvc p WaitFo
94. e Environment NewLine result t string obj Ndis80211ReceivedSignalStrength ToString Environment NewLine result END Environment NewLine if result result No active WI FI adapters found return result Trim catch Exception ex Console WriteLine ex StackTrace MessageBox Show ex Message Search Error MessageBoxButtons OK MessageBoxIcon Error return string Empty public static string getMACString System Byte mac string strMAC foreach Byte b in mac StrMAC b ToString X PadLeft 2 strMAC strMAC TrimEnd return strMAC De public static void ScanAvailableNetworks ManagementClass mc new ManagementClass root WMI MSNDas 80211 BssIdbistScan mull ManagementObject mo mc CreateInstance mo Active true 189 16 7 wW mo InstanceName Adapter 3 mo UnusedParameter mo Put 0 public static void GetAvailableNetworks string result try f ScanAvailableNetworks Making the search string scope root WMI string query SEL active true ManagementObjectSearcher mos new ManagementObjectSearcher scope query ManagementObjectCollection moc mos Get Enumerator moe MoveNext moe MoveNext Adapter name Console WriteLine Adapter moe Current Properties InstanceNam
95. e Value Number of available wirel Console WriteLine Availabl moe Current Properties NumberOfItems Value ManagementBaseObject objarr ManagementBaseObject moe C moc Getl foreach ManagementBaseObject qu One SSID per line SSID PropertyData ssid queryObj PropertyData mac queryObj Properties Ndis80211MacAddress PropertyData rssi queryObj string strSSID Encoding ASCII GetString System Byte ssid Value Trim string strMAC ECT FROM MSNdis 80211 BSSIList WHE ManagementObjectCollection ManagementObject Atheros AR5001X Mini PCI Wireless Network 0 Enumerator moe ss networks wireless networks urrent Properties Ndis80211BSSIList Value ryObj in objarr name of nerwork Properties Ndis80211Ssid Properties Ndis80211Rssi End N0 getMACString System Byte mac Value BitConverter ToString System Byte mac Value uint RSSI UInt32 rssi Value iat RSSI_OFFSET 121 double dBm 10 Math Log10 1 0000 RSSI RSSI OFFSET if RSSI dBm 130 result MAC strMAC tdBm dBm UNERSSI RSSI ToString X PadLeft 2 0 190 Environment NewLine Console Write result Console WriteLine ssid Name strSSID type ssid Type Console WriteLine mac Name strMAC type ti
96. e Ue ibt peciit ai ur ed 75 Appendix F Wi Fi Kalman Filter Results eerie erepta aao aec eal a ied eda 78 Appendix G Wi Fi Kernel Method MATLAB Codes eene 79 Appendix H EKF Time Update MATLAB Codes eee 81 Appendix I EKF Measurement Update MATLAB Codes eee 82 Appendix J EKF Initialization MATLAB Codes eene 83 Appendix K Angle Clip MATLAB Codes NAL PN rua dd anao 84 Appendix L Ground Truth MATLAB Codes eese nennen 85 Appendix M Get IMU Data MATLAB Codes naaasar aaa 87 Appendix N Ground Truth C Code dott e dete ated tui Naan a 88 Appendix O 1 Robot Remote Control Client ER1Navigator Models 89 Appendix OL Ie ROBO avd aoro taa edo Ded deseo e dedu Ba a a eee 89 Appendix O 1 2 Map Va G eate uc e e t e utes 91 Appendix O 1 3 ER1RemoteConnection java eese 94 Appendix O 2 Robot Remote Control Client ER1Navigator Controllers 95 Appendix O 2 1 ERINavigatorFrame java eese enne 95 Appeudix 0 22 ERIController 8Vd c s eec sto ctp eaa mapa aaa talag 97 Appendix 0 2 3 MapConitroller Java esae ed doas on SAA 98 Appendix 0 2 4 MoveControllerJjava a aa co decl aea Naa 99 Appendix O 2 5 RobotLocationController java eee 104 Appendix 0 2 6 LoggerController java
97. e controller to disabled timer new Timer interval create timer with specified interval timer Elapsed doUpdate Handle time interval elapsed event with doUpdate data new ArrayList Create array to stored past SensorData 138 interval event wi public SensorController int interval this interval interval set the timer interval updating false initialize controller to disabled timer new Timer interval create timer with specified timer Elapsed doUpdate Handle time interval elapsed th doUpdate data new ArrayList Create array to stored past SensorData This function gets called at the specified time interval and is repsonsible for reading the current data from the sensor abstract public void doUpdate object sender ElapsedEventArgs e Function to be called every time a new value is received public delegate void SensorDataReceievedDelegate SensorData data starts the polling of sensor data from the sensing hardware virtual public void startUpdating timer Start Start the timer which triggers doUpdate every time interval updating true sensor data updating enabled stops the sensor controller from polling hardware virtual public void stopUpdating timer Stop Start the timer which triggers doUpdate every time interval updating false sensor data updating disabled
98. e corridor The data that indicates the distance of the walls from the robot might be useful in this manner if the user instructs the robot to follow a certain straight way and the left and right ultrasound measurements change by a wide margin that proves that the robot is drifting It is possible to develop software that can order to robot to keep a fixed distance from the walls at the both side unless it is ordered to turn So the ultrasound sensors can be used to remove or at least decrease the effect of drift during the movement of the robot The sample code to get four ultrasound sensors to work at the same time can be found in Appendix C The problem of the ultrasound sensor is that its readings are not perfectly accurate and consistent When it displays the distance measured 17 inches for a few seconds it can suddenly jump to 243 inches which is the maximum range of it and cause inaccuracies with our measurements The usage of a more precise sensor such as an optical one is required at this stage Also since we need to return the ultrasound sensors that we borrowed from Robotics Department at WPI at the beginning of D Term we need to buy new sensors and this will cost around 80 74 Appendix E Kalman Filter Largely due to the constant variations in the Wi Fi localization techniques it is practical to combine Wi Fi localization measurements with a filter that in a sense smoothes out the measurements Although there are many smoot
99. e different sensors and applying the fusion algorithm on the sensors eeseeeeeeeeeeenen nennen 39 Figure 20 Robot location server interface showing the different sensors ss 40 Figure 21 Robot location server showing the different algorithms eeeesees 41 Figure 22 Ground truth application for logging the actual position of the robot with time 43 Figure 23 Performance of the dead reckoning algorithm green vs ground truth purple 45 Figure 24 Timing error analysis of the dead reckoning algorithm sess 46 Figure 25 Cumulative density function of the dead reckoning algorithm 47 Figure 26 Performance of the wi fi localization algorithm red vs ground truth blue 48 Figure 27 Timing error analysis of the wi fi localization algorithm eese 49 Figure 28 Cumulative density function of the wi fi localization algorithm 50 Figure 29 Performance of the Extended Kalman Filter algorithm blue vs ground truth black 51 Figure 30 Timing error analysis of the Extended Kalman Filter Algorithm 52 Figure 31 Cumulative density function of the Extended Kalman Filter 53 Figure 32 Timing error analysis of the algorithms Extended Kalman Filter wi fi localization and dead Bed Oni E
100. e explained by Wi Fi s slow response to instantaneous change as well as its short term variations Lastly with the EKF which takes advantage of the inertial system for short term tracking and Wi Fi Kernel method for long term tracking it displayed that ninety percent of the error is within 2 7 meters which is very good in terms of indoor localization Our fusion algorithm performed the best overall however future work can be implemented to further improve its accuracy Future Work Overall we accomplished most of our objectives in this project however we can further improve the Extended Kalman Filter algorithm First Particle filter is another widely used fusion algorithm for indoor localization In the future the performance of the Particle filter can be compared with Extended Kalman Filter to see which fusion algorithm performs better in indoor environment Secondly we can use the map of the building as a reference to improve the accuracy of the EKF Using the map we can identify the walls rooms and obstacles within the building in order to prevent the robot from collision and going through the walls Thirdly we have not fully tested how changing the covariance matrix of the EKF will affect the system as a whole The Kalman Filter is a smoothing filter and it smoothes out the noise and drift introduced by local sensors as well as global sensors so that it can combine the advantages of the various sensors However it is slow in response to
101. e increases the drift also increases Then there is an inversely proportional relation between the drift and velocity Taking the fact that both accurate velocity and positioning is required into consideration a step size 15 inches will be the most efficient case When the robot is controlled by using the keypad or joystick the robot will move 15 inches at each movement instruction 62 Appendix B Wi Fi Localization Flow Chart d Calculate Location Estimate Output Read in RSS in dB DATABASE disp x y x y pa Prob i xiy P 85 74 t m n l ls m access point sample l location Compute Sum of Squared Errors Compute for Probability for Each Sample and Location n Value m Knorm 2 Assume 7 1 Prob l c2 SSE n Y P t n 1 TE k 1 Compute Kernel for Each Sample Compute Average of Kernel Normalize Average Kernel and Location for Each Location Values 3gzSsE nD Kavg D aaa 1x Garo Kava TY Ka Knorr O MaxKarg D a 1 K n 1 Manipulate Variance Value o variance Figure 36 Wi Fi Flow Chart 63 Appendix C sonar c Code that is developed for Vex Ultrasonic Rangefinder include void main void Bu uiltIns H char result 10 TwoWheelDrive 2 1 use a two wheel drive robot 1 StartUltrasonic Drive 70 7 start ultrasonic sensor ranging 0 start robot driving forward GetUltrasonic l 7 continue wh
102. e to instantaneous change Furthermore it is also subjected to short term variations An IMU is used in this case to correct the readings of the Wi Fi for indoor applications In the study of Frank Krach Catterall and Robertson 3 the IMU has provided an improved performance of determining the position of the robot However all the sensors lOlPage integrated in the IMU possess a certain amount of drift Thus its performance is dependent on time and it starts to deteriorate as time increases While the odometer is good at providing linear velocity its estimation of angular motion is not quite good Thus the odometer should be compensated by other sensors to provide motion tracking of the object In the recent years many different algorithms that are used to combine the readings of various sensors have been studied The particle filter is a sophisticated model estimation technique based on simulation 4 Each particle in the filter is heavily relied on the number of samples When the number of sample is large enough the performance of a particle filter is better than the Extended Kalman Filter Extended Kalman Filter can be used to combine the readings from various sensors and to display them on a map in real time by estimating the initial position and heading The study of Frank Krach Catterall and Robertson 3 proves that Extended Kalman Filter can be a very useful method to track the indoor movement of a robot throughout a pre defined
103. eading StopUltrasonic 1 7 stop ultrasonic sensor reading 25 Page The 1 and 7 represent the ports that are being used by the Ultrasonic Rangefinder One represent the interrupt port one and seven represent the I O port seven Whenever the ultrasonic range finder reads a new value the interrupt is sent to Vex Controller Unit to update the data To display the ultrasonic reading on screen the command PrintToScreen result is used EasyC Pro program runs the code and outputs a hex file IFI Loader program exports the hex file onto the Vex Controller unit The ultrasound sensor is tested on the third floor of Atwater Kent Laboratory and the output values are compared with the readings of the laser sensor which is more accurate and expensive in order to understand how accurate the sonar sensor is The results of these comparisons are included in Appendix D 26 Page CHAPTER III LOCATION AND TRACKING ALGORITHMS The keys to our entire localization algorithm are to first find individual methods to achieve accurate results for each individual sensor Secondly we have to develop algorithms to integrate all our sensors to compute the position of the robot Extended Kalman Filter ma He Displacement Computer Kernel Method T m m Inertial Evolution Robot 1 Wi Fi Received Signal Measurement Unit AB Strength Gyroscope Figure 11 Overall description of the algorithms with an outline of the individual sampling frequencies of each
104. eak void ProcessRequest object listenerContext try f var context HttpListenerContext listenerContext string filename Path GetFileName context Request RawUrl string path Path Combine baseFolder filename byte msg context Response StatusCode int HttpStatusCode OK msg StrToByteArray getResponse filename context if filename Equals current jpg lock this if update_camera_frame fails t if fails gt 5 camera_connect fails 0 Console WriteLine CAMERA RECONNECTION ATTEMPT FileStream fs File OpenRead G C Ncurrent jpg msg new byte fs Length fs Read msg 0 int fs Length fs Close context Response ContentLength64 msg Length using Stream s context Response OutputStream s Write msg 0 msg Length 173 catch Exception public bool camera_c if matlab nu matlab Execute Files MATLAB R2006b ex Console WriteLine Request error ex onnect 11 return false addpath C Program matlab Execute LAB R2006b toolbox Files MAT string msg mat System Windows if msg Contains return false return true toolbox ERTOOlkits addpath C Program tcp udp ip lab Execute G erInit Forms MessageBox Show msg server is busy public bool camera disconnect if string msg
105. ement has a small amount of drift which will be crucial if we let the drift build up This is one of the factors we considered while implementing the EKF 2 2 3 Magnetometer for Sensing Initial Heading In addition to the gyroscope we also performed tests to verify the functions of the IMU s magnetometer We wanted to test its performance to see whether or not we can integrate it into the fusion algorithm to better track the angle The following table represents the results we achieved from our magnetometer evaluation Sample Number Mag X Mag Y Mag X Mag Y Mag X Mag Y Mag X Mag Y 1 0 03380 0 34790 0 31390 0 14820 0 44300 0 50610 0 23500 0 67990 DIE 0 03430 0 34810 0 31400 0 14720 0 44700 0 51650 0 21370 0 69820 3 0 03350 0 34610 0 31330 0 14890 0 44230 0 50380 0 20900 0 70210 4 0 03490 0 34780 0 31530 0 14550 0 44280 0 50540 0 20610 0 70330 5 0 03260 0 34880 0 31370 0 14760 0 44250 0 50700 0 20510 0 70540 6 0 03260 0 34780 0 31380 0 14690 0 44340 0 50550 0 20620 0 70460 Ft 0 03360 0 34620 0 31440 0 14530 0 44310 0 50500 0 20630 0 70240 8 0 03290 0 34850 0 31460 0 14710 0 44240 0 50500 0 20740 0 70470 9 0 03360 0 34850 0 31580 0 14660 0 44210 0 50530 0 20760 0 70330 10 0 03400 0 34910
106. endix A 2 4 Ultrasound Sensors for Correcting Heading In order to obtain more accurate position and velocity data the computer interface of the ER 1 robot should be detecting the coordinates of the robot as correctly as possible The usage of 23 Page sensors provides more data to the user interface such that when the incoming data from IMU ER 1 Wi Fi and the sensors is combined through an algorithm the user of the robot interface will know where the robot is exactly The integration of sensors into the robotics system is not a brand new research area There are researches concentrating on sensor integration into robotic systems and one of them is SLAM for Dummies Riisgaard Blas 7 SLAM means Simultaneous Localization and Mapping The purpose of the paper is to give a full tutorial about SLAM field Throughout the tutorial the integration of an optical sensor into an ER 1 robot is explained in details This particular paper would become fairly useful for our Indoor Localization Project since we are also working on to integrate sensors into an ER 1 robot and to increase the accuracy of positioning Low Cost Optical Indoor Localization for Mobile Objects without Image Processing Salomon Schneider Wehden 8 is another paper that concentrates on integrating sensors into robotics systems The purpose of the paper is to explain the weaknesses and disadvantages of GPS systems when they are used indoors GPS systems perform limited performa
107. ener Stop public static byte StrToByteArray string str System Text ASCIIEncoding encoding new System Text ASCIIEncoding 178 return encoding GetBytes str public void setER1Controller ER1Controller erlController this erlController erlController public ER1Controller getERlController return erlController Appendix P 2 9 Form1 cs using System using System Collections Generic using System ComponentModel using System Data using System Drawing using System Ling using System Text using System Windows Forms using ERlLocationListener Controllers using System Management using System Management Instrumentation using WIA namespace ERlLocationListener f public partial class Forml Form delegate void SafeSetTextCallback TextBox button string text delegate void RenameButtonCallback Button button string text delegate void ListAddDataCallback ListBox listData Models SensorData data delegate void DataGridAddRowCallback DataGridView dataGridView Models SensorData data delegate void DataGridAddObjectRowCallback DataGridView dataGridView object data delegate void SetTextCallback string text delegate void UpdateListCallback Models SensorData data ur private RemoteCommunicator rc private FRIController eriController private IMUController imuSensor private WiFiController wifiSensor private S
108. eplay else logger stopReplay btnReplay setText Start Replay update start button updateStartButtonLabel public void updateReplayButtonLabel 116 Appen if logger isReplay f btnReplay setText Stop Replay else btnReplay setText Start Replay dix 0 3 6 MapPanel java package views import import import import import import import import import import import import public ZOOM INIT ava awt ava awt event ava awt geom AffineTransform ava awt image AffineTransformOp ava awt image BufferedImage ava io File Wa CJ Joa Joa Ju ca javax imageio ImagelO javax swing Javax swing event controllers MapController models Robot models class MapPanel extends JPanel implements ChangeListener private MapView mapView private RobotView robotView private JSlider zoomSlider private JCheckBox detailCheckBox private JLabel headingLabel private BufferedImage mapImage private Image zoomMapImage private static final int ZOOM MIN 0 private static final int ZOOM MAX 100 private static final int ZOOM INIT 25 private int zoomWidth 1 private int zoomHeight 1 public MapPanel Map map Robot robot MapController mapController mapView new MapView map robotView new RobotView robot zoomSlider new JSlider JSlider HORIZONTAL ZOOM MIN ZOOM MAX zoomSlider s
109. erride void startUpdating Start Sonar Sensor base startUpdating Forml mainForm Forml Forml ActiveForm mainForm SafeRenameButton btnStart Stop Updating Sonar public override void stopUpdating Stop Sonar Sensor base stopUpdating Forml mainForm Forml Forml ActiveForm mainForm SafeRenameButton btnStart Start Updating Sonar private void Init Sonar Sensor string COMPort COM12 int baudrate 115200 sonar new SerialPort COMPort baudrate private void Start Sonar Sensor sonar Open private void Stop Sonar Sensor sonar Close private string Get Sonar Data rey return sonar ReadLine 158 catch Exception ex Console WriteLine Sonar Error ex Message n StackTrace ex StackTrace return ERROR Read the current values from the IMU and store it override public void doUpdate object sender System Timers ElapsedEventArgs e Called every time interval Run MATLAB Script to Read Ultrasonic Distance Values SensorData newData SensorData NoSensorData string response NO RESPONSE try lock this f Create Ultrasonic Data object from the Sonar s output variables string sonarData Get Sonar Data Console WriteLine Sonar Read sonarData newData SonarData Parse sonarData Console WriteLine Sonar Parsed newData
110. es view checkbox is changed rce equals detailCheckBox 0 ider ilView Box Label d he Robot s location nt Graphics g d nt g isSelected if detail view mode is t g Render Map as 119 else g drawImage zoomMapImage 0 O null Display map from image Display the Robot s location on Map robotView paint g headingLabel setText Heading getRobot getAngle public models Robot getRobot return robotView getRobot Appendix 0 3 7 MapView java package views import java awt import javax swing JComponent import models public class MapView extends JComponent f private Map map private double PixelsPerCM 0 25 public MapView Map map this map map this setSize CMToPixels map getWidth CMToPixels map getHeight public Map getMap return map private int CMToPixels double cm return int cm PixelsPerCM public void setZoom double zoom PixelsPerCM zoom this setSize CMToPixels map getWidth CMToPixels map getHeight 120 public void paint Graphics 9 Set current drawing color g setColor Color BLACK Draw all the lines for int i 0 i map getLines size i Line line map getLines get i g drawLine int line getStartPoint getX PixelsPerCM int line getStartPoint getY PixelsPerCM int line getEndPoint getX
111. etMajorTickSpacing 10 zoomSlider setMinorTickSpacing 1 zoomSlider setPaintTicks true zoomSlider addChangeListener mapController 117 zoomSlider addChangeListener this mapController setMapView mapView mapController setRobotView robotView Create the checkbox and action listener detailCheckBox new JCheckBox Detail View detailCheckBox addItemListener new ItemListener public void itemStateChanged ItemEvent e detailView detailCheckBox isselected n detailCheckBox new JCheckBox Detail View detailCheckBox addChangeListener this detailCheckBox addChangeListener new ChangeListener public void stateChanged ChangeEvent e detailView detailCheckBox isSelected ee headingLabel new JLabel setBackground Color WHITE this setSize mapView getSize this add zoomSlider Read the detailed map image of the building String mapFileName AKL FL3 jpg File imgFile new File mapFileName System out println imgFile getAbsolutePath try mapImage ImageIO read imgFile catch Exception e System out printlin e zoomWidth mapImage getWidth zoomHeight mapImage getHeight Flip the image vertically AffineTransform tx AffineTransform getScaleInstance 1 1 tx translate maplImage getWidth null 0 AffineTransformOp op new AffineTransformOp tx AffineTransformOp T
112. etValue 0 Pitch double eulerAngle GetValue 1 Roll double eulerAngle GetValue 2 public IMUData Array accel Array gyr Array mag DateTime timestamp base timestamp AccelX double accel GetValue 0 AccelY double accel GetValue 1 AccelZ double accel GetValue 2 GyrX double gyr GetValue 0 GyrY double gyr GetValue 1 GyrZ double gyr GetValue 2 MagX double mag GetValue 0 MagY double mag GetValue 1 MagZ double mag GetValue 2 public double AngularVelocity get return GyrZ set GyrZ value public double YawAngle get return Yaw set Yaw value public double MagXVal f get return MagX set MagX value public double MagYVal get return MagY set MagY value override public string ToString 132 string str AccelX AccelX AccelY AccelY AccelZ T AccelZ Mn Str Gyr Us GyrX c OU GYE a c4oOGyrY 4t 7 QGyrAt Gb GyrA F TONES str MagX Magx Magy MagY Maga Magz n str TIMESTAMP this Timestamp ToString HH mm ss return str override public object ToArray f return new object AccelX AccelY AccelZ GyrX GyrY GyrZ MagX MagY MagZ Yaw Pitch Roll Timestamp TimeOfDay ToString Converts ER1Postion to array of objects for Matlab public do
113. ewState Typ Save new EKF State ReceivedData Add newState currentState State newState State e G urrentState Covariance newState Covariance Display the updated EKF State and Covariance SensorUpdateReceived newState return catch System Runtime InteropServices COM Con Exception Display Error string message Matlab Command Exception comEx scriptName n message Matlab Response response n message Exception comEx Message n MessageBox Show message sole WriteLine EKF Time Update FAILED t response public static void WiFi Measurement Update SensorData wifiData lock c ER1 urrentState Extract WiFi data Position wifiPos null wifiPos ER1Position wifiData 169 gaten InvalidCast throw new Argument Exception ex Exception EKF Invalid WiFi Data wifiData Get the current state and covariance x P and give to MATLAB Array State Covariance WiFiPos dummy dummy2 dummy3 State currentState State3 Covariance currentState Covariance3 WiFiPos new double wifiPos XPos wifiPos YPos dummy new double State Length dummy2 new double State Length State Length dummy3 new double WiFiPos Length Use matlab to Update Current EKF State a
114. formances of the three algorithms dead reckoning Wi Fi Kernel method and the Extended Kalman Filter The first plot is the timing error analysis comparing all three algorithms Timing Error Analysis of EKF ER18IMU and Wi Fi ENF ER18IMU Error in meters 0 100 200 300 400 500 600 700 800 900 1000 Time in seconds Figure 32 Timing error analysis of the algorithms Extended Kalman Filter wi fi localization and dead reckoning In the plot above the first thing to note is that the dead reckoning algorithm performed the worst It illustrated the largest amount of error Thus it is only reasonable to compare the EKF with the Wi Fi Kernel method From the plot we can see that the error of Wi Fi is relatively large at several areas On the other hand the EKF only displayed a large error only at one time frame However both the EKF algorithm and the Wi Fi Kernel method were able to correct themselves even with the large errors After observing the timing error analysis we used the CDF plots from the previous sections to compare the error rates of the three algorithms 54 Page CDF plot EKF ER18IMU and Wi Fi EKF ER1 amp IMU WiFi o 0 6 Probability o o o w gt e o ho 0 0 5 1 1 5 2 2 5 3 3 5 4 45 5 Error in Meters Figure 33 Cumulative density function of the three algorithms extended Kalman filter wi fi localization and dead reckoning From the above CDF plot we c
115. getValueIsAdjusting velocity velocitySlider getValue 99 tvelocity n try robot getClient sendCommand set v catch TODO Auto generated catch block el printStackTrace Exception el Override public void keyPressed KeyEvent event if robot connected updateControlLabel false No Connection to ER1 return if lastEvent null amp amp lastEvent getKeyCode vent getKeyCode return lastEvent event switch event getKeyCode case KeyEvent VK_UP upPressed break case KeyEvent VK DOWN downPressed break case KeyEvent VK_LEFT leftPressed break case KeyEvent VK RIGHT rightPressed break default updateControlLabel true break Override public void keyReleased KeyEvent arg0 if robot connected updateControlLabel false No Connection to ER1 return lastEven switch arg0 getKeyCode case case case case Key Key Key Key E O Event Event Event Event ull VK UP upReleased break VK DOWN downReleased break VK LEFT leftReleased break VK RIGHT rightReleased break updateControlLabel true Clear movement from control label Override public void keyTyped KeyEvent arg0 TODO Auto generated method stub public void upPressed 100 updateControlLabel true Mo
116. h ArgumentException ex Use last position position ERlPosition data data Count 2 Console WriteLine Invalid ER1Position Response ex Message lt position Timestamp gt else Console WriteLine ER1IController cannot update ERI because NOT CONNECTED return Calculate Velocity between current and last position SensorData lastData CurrentData ERlPosition lastPos lastData Equals SensorData NoSensorData null ERlPosition lastData 143 position Velocity this getVelocity position lastPos Calculate the Angle from Direction of Velocity if SensorData isValidSensorData position amp amp SensorData isValidSensorData lastPos Make sure last 2 positions valid double dy position YPos lastPos YPos double dx position XPos lastPos XPos position Angle Math Atan2 dy dx Store this recevied sensor data along with the rest of the past data ReceivedData Add position if new data was recieved and the callback is defined pass new data to callback if position Equals SensorData NoSensorData amp amp this SensorUpdateReceived null SensorUpdateReceived position else Otherwise there was NoSensorData or no callback was defined Console WriteLine ER1IController data position SensorDataReceived SensorUpdateReceived n Export all logged
117. he ER1 robot and receive and handle responses The ER1 Location Server sends position commands to the ER1 s remote API at every specified update interval and logs them with their timestamp This connection is also used to send and received commands to the robot for moving setting speed and other Remote API commands provided by Evolution Robotics 39 Page ER1 Location Listener IM ERI Remote API IMU Data WFi Data Sonar Algorithm EKF Camera Stan Updating WiFi Stop WZC Export to MAT 19762107381535 4796 579437413 0 1976 2107361595 4796 673437413 0 1976 7107361535 4796 679437413 0 1976 2107361595 4796679437413 0 1976217361535 4796 679437413 0 1976 2107361595 4796 679437413 0 1976 2107361535 4796 679437413 0 1982 239042718 4799772452571 0 1967142365523 4834 024533177 0 1952887342534 4823 832225455 0 1962987342534 4823832225455 0 1952887342534 4823 832225455 0 1952887342534 4823932225455 0 1952887342534 4873632225458 0 1952887342534 4823 832225455 0 1952887342534 4823 832225455 0 1952887342534 4823 832225455 0 lo lo lo o o I o 0 lo lo lo o lo o D 2247 55 0837500 224753 8437500 2247 52 5000000 2247 51 1250000 2247 48 8593750 22 47 48 5537500 20 47 47 3437500 224746 224744 7343750 224743 4697500 224742 1875000 2247 40 9218750 2247 33 6562500 22 47 3
118. he sum of squared errors for each sample and location using the received signal strength readings We recorded the received signal strength measurements of 10 locations on the third floor of Atwater Kent Laboratory At each location we collected 24 training samples from 65 different access points Using the received signal strength measurements from the object we first calculated the sum of square errors as described in 9 Next we conducted the calculations depicted on the next figure 28 IPage Compute Kernel for Each Sample Compute Average of Kernel Normalize Average Kernel and Location for Each Location Values E E Kag V2ro Kayg D 2x 1 Knorm Max Ka K n l Manipulate Variance Value o variance Figure 13 Creating and normalizing the Kernel for probability estimate Using the SSE data and the variance value we created the average Gaussian Kernel at each of the 12 locations In our experiment we used a variance value of 0 02 In addition we had to tweak this value many times in order to achieve the most accurate localization estimations Next with the average Gaussian Kernel we normalized it and calculated the probability as shown in the next figure Calculate Location Estimate Output l disp x y CI y J Prob Grey i 1 Compute for Probability Knorm 2 Knorm Prob 1 Figure 14 Calculating the probability of the robot in each location using the Kernel 29 Page W
119. hen we calculated the probability we assumed that value of n to be 1 as suggest in 9 Once we have the probability values we can then use it to estimate the position of the object 3 2 Extended Kalman Filter for Sensor Fusion One of the best approaches to combining the IMU gyroscope readings the Wi Fi RSS measurements and the ER1 odometer data is by implementing the Extended Kalman Filter The Extended Kalman Filter EKF is an adaptive filter that models non linear movement and state transitions Since the Extended Kalman Filter is a nonlinear filter it is very practical to use it in our case because it can smooth the readings from our sensors to better track the robot s position Our EKF contains three stages the initialization stage time update stage and measurement update stage similar to 10 Measurement Update Correct O J B 29 Lilacs nna C Eredet 1 Compute the Kalman gain 1 1 T the ahead K PHT H P H V R VZ S f a nas 0 v 2 Update estimate with measurement zy 2 Project the error covariance ahead t Ec E EE k z ka h amp 0 P AP AC F W Q i WI 3 Update the error covariance Initial estimates for ig and P Figure 15 Overview of the processes involved in the Extended Kalman Filter algorithm The purpose of the initialization stage is to initialize the state and covariance With the state we can then process the data during the time
120. hing filters that we can use to satisfy our goal we particularly paid attention to the Kalman filter developed by R E Kalman In general the Kalman filter computes the mean of the state of process through recursive mathematical equations Furthermore the filter is extremely useful in estimating the past present and future states as suggested by 11 Another important note is that the Kalman Filter is a linear filter we will not be able to use this filter to combine all our sensors However we can use this filter to smooth the measurements of Wi Fi before using another algorithm to combine our three main localization sensors The whole idea of the Kalman Filter can be characterized by the following figure Measurement Update Correct ime U ce ip Time Update Predict 1 Compute the Kalman gain 1 Pais the state ahead K HE P HT HP HT R p f Ad Bu 4 2 Update estimate with measurement z 5 m NN REDE a pas T 2 Project the error covariance ahead Ng X K Z HXx P kc AP k A Q 3 Update the error covariance P I K H P Initial estimates for 3 and P Figure 41 Kalman Filter Model The filter starts by making a prediction using the past result noise covariance and state transition matrix Next the filter computes a measurement update based on the current An Introduction to the Kalman Filter by Greg Welch and Gary Bishop Page 6 75 measurement
121. hniques have been the interest of study for decades Until recently Global Positioning System GPS has been developed and widely used in commercial applications However its performance in indoor area is significantly deteriorated due to path loss and other signal degenerating agents Due to the inaccuracy of GPS in indoor applications there is an increasing demand for precise localization inside buildings Wi Fi based localization technology makes use of Received Signal Strength RSS to determine the position of an object inside a building One of the advantages of Wi Fi localization is that it uses an existing infrastructure which makes it an inexpensive sensor with extremely low power consumption Lastly since it is also a software based sensor it is far easier to debug test and modify as opposed to hardware based sensors With the emergence of the Global Positioning System it has always been a technological interest to precisely locate the position of objects Many large corporations today apply these localization methods into commercial applications such as portable GPS car navigation systems by TomTom or handheld GPS devices for hiking by Garmin However due to the inability to identify obstacles inside buildings GPS navigation is only effective in outdoor applications Hence many engineers today are currently working on different systems for indoor localization Methods such as Wi Fi localization was designed for this purpose Algorithms
122. i Fi data As shown from the above plot the regular Kalman Filter displayed much better tracking of the robot than the raw Wi Fi readings The filtered plot shows a smoother transition compared to the unfiltered one 78 Appendix G Wi Fi Kernel Method MATLAB Codes expected an the input matrix result function position Get WiFi Position MACs RSSISs 3 validate MAC RSSI arrays are same length check length MACs length RSSIs if check 0 disp there is a dismatch between the size end clc close all PATH PATH C ER1lLocationListener Matlab WiFi 3 this path should be changed index 0 temp index load mac address3 mac address sorted list b number APs DB length mac address data zeros number APs DB 1 data 100 counter 1 clock 1 while 1 status result system WiFi WRAPI Demo exe call should be changed number APs length RSSIs calculates how many access point for i 1 number APs for j 1 number APs DB sdelta sum abs MACList i mac_address j if strcmpi MACs i mac address j temp index clock i jl clock clock 1 end end end clock 1 data temp index 2 counter RSSIs temp index 1 5 take record of the RSSI counter counter 1 clear MACs clear temp_index clear RSS temp clear RSSIS result data load database4 11 load loc x 4 11 load loc y 4 11 oe oo this oo oo th
123. ile distance gt 15 sprintf result sd GetUltrasonic 1 7 Drive 0 0 Jf stop driving PrintToScreen result StopUltrasonic 1 7 Four sonar c include void main void char resul BuiltIns h char resu char resul char resul StartUltrasonic StartUltrasonic StartUltrasonic StartUltrasonic sprintf result sprintf resul sprintf result sprintf resul d t2 d GetUltrasonic d t4 d GetUltrasonic t1 10 lt2 10 t3 10 t4 10 1 11 start ultrasonic sensor ranging 2 12 3 13 4 14 G etUltrasonic NG GetUltrasonic Ne BWN H A wN e e HY HY na PrintToScreen result1 ks tToScreen result2 PrintToScreen result3 tToScreen result4 topUltrasonic 1 11 S StopUltrasonic 2 12 StopUltrasonic 3 13 StopUltrasonic 4 14 r r 64 Line follower c include Main h void main void int loop 1 unsigned int Line follower Connect light sensor to Analog Input 4 Light 30 Dark 1000 while loop 1 Line follower GetAnalogInput 4 PrintToScreen Line Follower d n int Line follower Wait 1000 65 Appendix D Laser Optical Line Tracker Sensors Here are several types of sensors which can be integrated into ER 1 robot varying by price ac
124. imestamp java util Date today new java util Date Time now new Time today getTime PositionLog curPos new PositionLog x y angle now PositionLog lastPos null if robotLog size gt 0 lastPos robotLog get robotLog size 1 log x y angle and timestamp if isLogging amp amp txtLogArea null txtLogArea append curPos toString Nt PositionLog calcVelocity lastPos curPos cm s n robotLog add curPos public void setLogArea JTextArea txtLogArea this txtLogArea txtLogArea Appendix 0 3 Robot Remote Control Client ER1Navigator Views Appendix 0 3 1 CameraPanel java package views 107 import import import import import import import public ava ava awt awt image BufferedImage Ws Ue CJ CJ Ju LW ava ava ava net MalformedURLException ava net URL JULIl g s util Timer javax swing class CameraPanel extends JPanel Image img Image lastImg Timer timer int interval interval to update frames in milliseconds String ipAddress 130 215 172 134 8888 localhost 8888 CameraPanel O this interval 500 this img Toolkit getDefaultToolkit createImage http ipAddress current jpg this timer new Timer timer schedule new FrameGrabTask this this interval class FrameGrabTask extends TimerTask CameraPanel cp
125. ing the ER1 Control Panel Remote API Telnet mode public string Hostname get return hostname set hostname value Port of the Laptop running the ER1 Control Panel Remote API Telnet mode public int Port return port port value gei sel public double getTotalDisplacement in in end this data Count start lastCount double displacement 0 double dt_total 0 int length end start if length lt 1 Console WriteLine ER1 no ER1 values over this Timelnterval ms to calculate displacement return 0 for int i start i lt end itt 145 displacement getDisplacement 1 dt total getTimeDifference i Console WriteLine ER1 Displacement in position displacement t cm using length start end 1 EKF samples over dt total ms Set the index of the last ER1 value used in Displacement lastCount end return displacement public double getCurrentDisplacement return getDisplacement this data Count 1 Get the displacement in position between the position at index sample and the position previous to that sample public double getDisplacement int sample if sample lt 2 sample gt this data Count return 0 ER1Position current ER1Position data sample ER1Position last ER1Position data sa
126. instantaneous changes in movement For the future work the second order Extended Kalman filter can be implemented to model the instantaneous velocity and heading change so that it will have better real time performance Most of our testing has been adjusting the parameters in the EKF and observing its resulting behavior A lot 57 Page of experiment and measurements remain open in this part of the research Only a first order model is implemented in this project where the effects of including the second order model have not yet been tested This can be done by modifying the state equation and the two update equations With the second order model we believe that we can better track instantaneous changes The last algorithm improvement is that we have yet to incorporate the sonar sensors We believe that with the aid of the sonar sensors we can better track the heading of the robot as well as using it for obstacle detection for autonomous movement Besides algorithm improvement we can also develop both autonomous and manned movement applications using our multi sensor location aware robot Some of the autonomous applications that we can implement include guided tours to provide better tour experience In our vision such applications can be employed at museums art galleries as well as universities In terms of manned movement applications telecommunication can be developed upon the algorithm we have already designed and these types of application
127. ions and advantages of each of the algorithms For the Dead Reckoning algorithm it displayed good results for short term movement whereas the Kernel method algorithm displayed good results for long term tracking But with the EKF which combines both of the previously discussed algorithms it performed the best overall by taking advantage of each of their pros The first subsection however will explain how we generated the ground truth plot Furthermore we will discuss the accuracy of the overall improvement of the position estimates when the EKF is used 5 1 Generating Ground Truth for Reference The first step to generating the ground truth plot was to develop an application that allows us to measure the time and coordinates of the position of the robot With the ability to track the time and real position of the robot we composed a small script that allows us to generate the ground truth plot The script requires the time when the robot stopped and started moving again In addition the positions at those two time frames are also really important With those data we assume linear velocity across a certain time frame to generate the ground truth plot The figure below illustrates the ground truth plot application 42 Page 22 40 52 7444642 Height 366px 3294cm 22 4121 1991813 22 41 30 2524094 22 41 53 6960002 22 42 11 7434550 22 42 23 6107540 22 42 37 7731109 22 42 46 5403318 22 44 14 7595549 22 47 20 412340 22 47 42 3027
128. is APs size database 1 3 54 access points y size database 2 3 30 training samples Z size database 3 3 10 reference locations points 1 t zeros APs y z 3D matrix as database noreading 85 p result sigma 0 02 qo eta 1 sse 0 knorm zeros 1 z prob zeros z points kgauss zeros APs z for m 1 1 points for k 1 1 z for i 1 1 y for 1 1 1 APs sse sse t p l m database l i k 2 end kgauss y k z exp sse 2 sigma 2 sqrt 2 pi sigma APs sse 0 end end orm k kgauss b k knorm knorm y kmax max knorm knorm knorm kmax ksum sum knorm prob m eta knorm ksum probsum sum prob end position Location Estimation prob loc x loc y function position Location Estimation probab x coor y coor num size probab route zeros 1 num 1 2 route count 1 for i 1 num 1 for j l num 2 route route count route route count probab j i x coor j route route countt1 route route countt1 probab j i y coor j end route count route count 2 end disp route 1 180 20 disp route 2 180 20 position route 1 180 20 route 2 180 20 80 Appendix H EKF Time Update MATLAB Codes oo TIME UPDATE EQUATI ONS predict x x k 1 f x k project ahead using last estimate P k 1 A k P k A k Q k function x P Time Update x P u
129. is back Double Parse this left Double Parse this right Double Parse public double Front RSSIs i Distance GetValue GetValue GetValue GetValue front of robot back of robot left of robot Fight of robot from from from from nce nce nce es base 0 1 2 3 anana double back double left double string back string left string front Dind back Split left Split right Split 1 nps D A 1 136 get return front set front value public double Back get return back set back value public double Left get return left set left value public double Right get return right set right value Converts SonarData to array of objects for Matlab public double ToMatlabArray return new double Front Back Left Right Timestamp TimeOfDay TotalMilliseconds override public string ToString string str Front Front Back Back Left Left Righty Right Xa str TIMESTAMP this Timestamp ToString HH mm ss return str override public object ToArray return new object Front Back Left Right Timestamp TimeOfDay ToString public static SensorData Parse string data string front back left right Tl string part
130. isplay Location Sensors Inertial Measurement Unit Wheel Encoder Wi Fi Scanning Gyroscope Angular Velocity rad s Velocity cm s BSSID MAC Magnetometer SN RSS dBm Figure 16 Architecture overview identifying the sensors as well as the server to client platform with real time remote control video stream and map display 35 Page The algorithms which include post processing of sensor data Kernel Method and Extended Kalman Filter are implemented in MATLAB and integrated into the robot server application 4 2 JAVA Client Interface The purpose of the Client Side JAVA Interface is to first receive the end result of the localization algorithm using the raw data from various sensors located on the ER1 Robot Next it then combines the localization information with the map of the building to plot the robot s position and orientation throughout the building in real time The JAVA interface also allows the user to remotely navigate the ER1 Robot throughout the building and receive a real time video feed Robot Client Figure 17 Robot client side of the architecture with remote control video stream and location map display 36lPage The ERI Robot s position is in units of centimeters and the angle of the robot s heading directions is in units of degrees These values are logged over time and visually displayed by converting the units from real units to pixels using a zoom factor controlled by a slider bar The a
131. k the consistency of the movement To record the velocity the time interval for each movement is recorded at first Then the distance that the robot travelled is recorded by both the computer and by actually measuring it with measurement tool The distance is divided by time to obtain the velocity of the robot The robot is tested in CWINS Lab and it is instructed to move 5 inches forward 22 Page Casel cwins lab PENE cometer USE eles tiny comp velocity cm s 8331100 5as 93 6 0 0 EE 525 0 1 5 1 mean 11 237 5 615 5 19 Table 2 Move 5 inches instruction test results for the robot The table above shows the measured distance in the first column Five inches is equal to 5 2 54 12 7 centimeters It is obvious that the robot is not travelling this distance The column comp cm indicates the position coordinates from the JAVA interface The first coordinate is x and the second one is y coordinate Dividing measured and computer indicated distance by time measured and comp velocity values are calculated The following four cases will be the measurement of the robot s movement under 10 15 25 and 50 inches conditions For each of those tables the columns will be the same Measured cm Actual distance covered by robot measured by optical ruler Comp cm The robot s position coordinates from ER 1 Measured Velocity and Comp Velocity These tables are included in App
132. ld provide a better angle measurement than the quaternion However in order to derive the angle readings using the gyroscope we have to integrate the gyroscope values Note that the gyroscope output the rate of turn in rad s so we have to integrate it The next figure shows our results for the new values MTi and MTx User Manual and Technical Documentation Pages 14 amp 19 18IPage Gyroscope vs time 100 100 200 30 Angle in degrees 400 500 600 D 50 100 150 Time in seconds Figure 7 Gyroscope readings of right turn made around the third floor of Atwater Kent Laboratory Each turn produces an approximately 90 degree change in the angle Furthermore we can also see that each right turn produces a negative change in the angle The first 90 degree change was produced by a left turn Our next test involves testing a series of left turn measurements The next figure illustrates the result for a series of left turns Gyroscope vs time 350 300 250 200 150 100 Angle in degrees 50 20 40 60 80 100 120 Time in seconds Figure 8 Gyroscope readings of left turn made around the third floor of Atwater Kent I9 Page Similar to the previous results the above figure achieved an approximate 90 degree change which is exactly what we wanted Since we have fixed the angle measurements of the IMU we can proceed to integrating the IMU and other sensors Another note is that the angle measur
133. let CMT get next data bundle h cmtGetNextDataBundle retrieve the data inertialData h cmtDataGetCalData deviceId 3 get latest calibrated data from buffer eulerAngle h cmtDataGetOriEuler deviceId get latest orientation data from buffer inertialData double inertialData data values can be converted to double for easy use in Matlab eulerAngle double eulerAngle data values can be converted to double for easy use in Matlab disp inertialData 87 Appendix N Ground Truth C Code using System using System Collections Generic using System ComponentModel using System Data using System Drawing using System Ling using System Text using System Windows Forms namespace GroundTruther public partial class Forml Form f private double PixelsToCM 180 0 20 0 public Forml f InitializeComponent labell Text Width pictureBoxl Width px pictureBox1 Width PixelsToCM cm n labell Text Height pictureBoxl Height px pictureBoxl Height PixelsToCM cm n labell Text nPixelsToCM 180 20 private void btnLogTime Click object sender EventArgs e dataGridViewl Rows Add DateTime Now TimeOfDay private void pictureBoxl Click object sender EventArgs e Point pos pictureBox1 PointToClient Cursor Position int xPos pos X int yPos pos Y if chkUseRealUnits Checked
134. lgorithm used for the positioning algorithm can be easily swapped out from the client to enable easy testing of various algorithms combining any of the connected sensors The JAVA interface is also useful for logging the raw data from the ER1 Robot s sensors as it moves throughout the building This data will be further analyzed using MATLAB to develop an effective and efficient algorithm to combine the raw data from the various sensors to achieve the most accurate localization results There are three main components in our JAVA interface The connection panel allows the client to define the connection information and establish a line of communication between the client interface and the robot control server Another main component is the map display which combines the end result position and the map of the building to display the robots current location to the user Lastly the third component is the remote control which focuses on allowing the user to remotely move the robot using the arrow keys on the keyboard An HTTP protocol will be used to allow the server side containing the ER1 sensor control system and algorithm to communicate with the client side remote control and location mapping system Additionally our architecture also allows for two way communication between two different people at two different locations The purpose of this is mainly for the application side of our project The following figure illustrates the current display of o
135. lts are simply unacceptable Next we transition to testing the Wi Fi Kalman Filter results 5 3 Performance of Wi Fi Localization Algorithm Upon completion of the algorithm that we are using we proceeded to testing its results So first we designed a test route with predefined coordinates and then measured the received signal strengths at each point of the test route Then we passed the measured RSS into our algorithm to see our results The following illustrates the predefined test route as well as the position estimations from our algorithm 47 Page Wi Fi vs Ground Truth Figure 26 Performance of the wi fi localization algorithm red vs ground truth blue From the above figure it is quite clear that there is some error in position estimations There are several areas in which Wi Fi would simply estimate an entirely wrong position In general Wi Fi was able to track the movement of the test route however we do not know to what extent is the accuracy of Wi Fi Similar to the previous section the first form of analysis we performed is the timing error analysis which is shown on the following figure 48lPage Error Analysis of Wi Fi Kernel Algorithm Error in meters 0 200 400 600 800 1000 1200 Timeins Figure 27 Timing error analysis of the wi fi localization algorithm Unlike the dead reckoning algorithm the Wi Fi Kernel Method displayed errors no greater than 8 meters Furthermore the Wi Fi data also shows that e
136. mac Type Console WriteLine rssi Name dBm type t mac Type Console WriteLine Encoding ASCII GetString System Byte queryObj Propert ies Ndis80211Ssid Value Other info Ndis80211Ssid Service Set Identifer array of uint8 byte Ndis80211MacAddress array of uint8 byte Ndis80211Rssi Recieved Signal Strength Identifier uint32 Must be converted to dBm my catch ManagementException e Console WriteLine An error occurred while querying for WMI data e Message MessageBox Show result private void btnStartUpdatingEKF Click object sender EventArgs e Console WriteLine START UPDATING EKF Not needed private void btnInitEKF Click object sender EventArgs e Gather 10 wifi samples wifiDB Clear wifiDB SamplesToRead 10 wifiDB onAllWiFiSamplesReceived EKFAlgorithmClass Initialize State 2 EKFAlgorithmClass Initialize State 2 private void btnEKFExportMatlab Click object sender EventArgs e EKFAlgorithmClass ExportToMatlab private void btnInitCamera Click object sender EventArgs e 191 camera return imuSensor Matlab Execute addpath C Program Files MATLAB R2006b toolbox ERToolkits imuSensor Matlab Execute addpath C Program Files MATLAB R2006b toolbox tcp udp ip BJ
137. ment will increase until the covariance values are decreased to a certain point The script for this stage 311Page can be found on Appendix J With the establishment of the initialization stage we proceeded to developing the time and measurement update stages 3 2 2 Time Update Process During the time update phase we update the state using only the gyroscope and the odometer as previously mentioned The important prediction equations are as follows The codes for our design and implementation of this part of the EKF are in Appendix H X f x 1 uz 0 0 P7 AP aA B4EL GB Qr X For simplicity reasons 7 4 and uy are typically set to zero as suggested by 4 However a possible function to combine our three sensors is the following fe Xp44 Xp ADycos D fy Yea Ye AD sini B Eq 4 fo 9x41 0x A0 Since we are combining Wi Fi ER1 and IMU we will achieve measurements in terms of x y angle velocity and angular velocity Using velocity and angular velocity we can then use the function in the figure above to predict where the next position is Also the state function is only used in the prediction phase to predict where the object is located better The last important feature of the prediction step is the transition matrix A ae fe fe 2 2 ap 1 0 AD sin 6 AT l0 1 AD cos 9 Lab ate Of fo lo 0 BER OQ Xj Qyk 00 Xk Kiriy and Buehler Page 4 32 Page In the EKF the st
138. ment the line tracker sensor The sample code for line tracker sensor can be found in Appendix C Figure 37 Vex Line Tracker Sensor 69 VEX Ultrasound Sensor Test Results The output values of the Vex Ultrasonic Rangefinder are very steady The refresh rate of the reading is 1 per second and the baud rate is 115200 In order to determine the efficiency of the Ultrasonic Rangefinder the distance readings from the rangefinder are compared with the readings from a laser measurement tool The results of the comparison are shown below Sonar Rangefinder Laser Measurement Tool Pros Has computer interface Much more accurate Mire steddy values a up to three significant Portable Portable Long battery life Long battery life Much cheaper More resistant to shocks Easy to develop Longer range around 30 m Cons Short range No computer interface Less accurate No measure below 0 6 m Not durable to shocks Much more expensive Measurements Device placed on the left side of robot Values vary between 28 33 inches which is between 71 83 cm While the robot is turning these values do not change considerably Values vary between 90 92 cm While the robot is turning these values do not change considerably Device placed on the right side of robot Values vary between 28 33 inches which is between 71 83 cm While the robot is turning these values do not change considerably
139. mple 1 double dx current XPos last XPos double dy current YPos last YPos return Math Sqrt dx dx dy dy public double getVelocity ERlPosition currPos ER1Position lastPos Need at least two position readings to calculate velocity if currPos null lastPos null return 0 Calculate the distance and time span between positions TimeSpan dt currPos Timestamp TimeOfDay Subtract lastPos Timestamp TimeOfDay double dx double dy currPos XPos lastPos XPos currPos YPos lastPos YPos if dt Milliseconds lt 1 return lastPos Velocity Calculate velocity components and vector double Vx double Vy double ve dx dt TotalSeconds dy dt TotalSeconds locity Math Sqrt Vx Vx Vy Vy return ve public double locity getXVelocity Need at least two position readings to calculate velocity if this data Count 2 return 0 146 Get the current and last position to calculate velocity ER1Position currPos ER1Position this CurrentData ER1Position lastPos ER1Position this data this data Count 2 Calculate the distance and time span between positions TimeSpan dt currPos Timestamp TimeOfDay Subtract lastPos Timestamp TimeOfDay double dx currPos XPos lastPos XPos return dx dt TotalSeconds public double getYVelocity Need at least two position readings to calculate vel
140. must be conducted to further analysis the magnetometer 21 Page 2 3 Odometer for sensing velocity In order to keep track of the robot s location the user interface should display the position and velocity readings accurately The major problem with the movement of the robot is the drifting problem The robot itself has a built in position sensor that transmits the robot s coordinates to the user interface but it is not possible to say that these readings are hundred percent accurate since the robot is drifting while calculating its location The importance of accurate position reading is to provide the user the ability to control the robot better Without correct coordinates the user would not know where the robot exactly is and this issue may cause the robot to hit the walls metal structures humans etc In order to see how the robot is actually moving various data is collected from the movements of the robot under different conditions The robot is tested in CWINS Lab The user interface that we have developed through JAVA allows us to give move and position commands to the robot Sample syntax for the robot to move 5 inches forward is move 5 i The robot is instructed to travel five different distances 5 10 15 25 and 50 inches The reason to work on a variety of distances is to observe whether the amount of the drift changes under different conditions Besides the position reading the velocity of the robot is also recorded to chec
141. nStr robot getClient sendCommand getPosition if positionStr startsWith IOException System out println Connection error return false Parse ER1 Data from Server String peices if peices length 4 System out println Position error return false positionStr split n positionStr positionStr String xPos peices 0 replace xPos trim String yPos peices 1 replace yPos trim String angl peices 2 replace angle trim String time peices 3 replace Timestamp trim Print TOM Time of Measurement 97 and TOA Time of Arrival Date toa new Date System out println TOM time Nt TOA toa double x Double parseDouble xPos in units of cm double y Double parseDouble yPos in units of cm double angle Double parseDouble angl in units of degrees robot updateLocation x y angle Position Logging if logger isLogging logger logLocation x y angle return true Override public void run TODO Auto generated method stub updateRobotPosition mapPanel repaint Appendix O 2 3 MapController java package controllers import javax swing JSlider import javax swing event ChangeEvent import javax swing event ChangeListener import views public class MapController implements ChangeListener private MapView mapView private Rob
142. nUpdateRecieved EKFAlgo SensorUpdateReceived this EKFUpdateRecieved EKFAlgorithmClass SensorUpdateReceived this EKFUpdateRecieved Initialize HTTP Remote Interfac rc new RemoteCommunicator ref erlController ref imuSensor ref ref positionAlgo i Timer frameTimer new Timer frameTimer Interval 500 frameTimer Tick new EventHandler frameTimer Tick frameTimer Start public void camera try string cameraDev iRez CommonDialogClass classl new CommonDialogClass WIA ImageFile image classl1 ShowAcquireImage WiaDeviceType UnspecifiedDeviceType 180 WialmageIntent ColorIntent WialmageBias MinimizeSize FormatID wiaFormatJPEG false true true Device d classl ShowSelectDevice WiaDeviceType CameraDeviceType true false DeviceManager manager new DeviceManagerClass Device d null foreach DeviceInfo info in manager DeviceInfos if info DeviceID cameraDev d info Connect break Item item d ExecuteCommand CommandID wiaCommandTakePicture foreach string format in item Formats WIA ImageFile imageFile item Transfer format as WIA ImageFile string filename c current jpg if string IsNullOrEmpty filename false imageFile SaveFile filename catch Exception e Console WriteLine e Message Console WriteLine e StackTrace
143. name this name name public void addLine Line line 92 f lines add line private int feetToPixels float feet return int feet CMPerFoot PixelsPerCM Adds a line to the map params in unit of cm param x1 cm ES Gparam yl cm param x2 cm param y2 cm s ab Sk Ab af public void addLine double x1 double yl double x2 double y2 lines add new Line x1 yl x2 y2 Adds a line to the map params in unit of feet param x1 Feet param yl Feet Gparam x2 Feet param y2 Feet XA XA 0X y public void addLineInFeet double x1 double yl double x2 double y2 lines add new Line x1 CMPerFoot yl CMPerFoot x2 CMPerFoot y2 CMPerFoot public ArrayList Line getLines return lines FileWriting http www rgagnon com javadetails java 0470 html public boolean writeFile String filename Create output stream FileOutputStream fos null try fos new FileOutputStream filename catch Exception e return false Create XML encoder XMLEncoder xenc new XMLEncoder fos Write object xenc writeObject new Map this 93 return true public boolean readFile String filename Create input streams FileInputStream fis null try fis new FileInputStream filename catch Exception e return false Create XML decoder XMLDecoder xdec new
144. nce and they induce costs that are far from being acceptable when they are used indoors The study in this paper basically tries to integrate an optical device into mobile objects to increase the localization precision Following the studies and results of the researches above we can perform the task of integrating a sensor into an ER 1 robot to increase efficiency of the robot As a result of integration the cost of performance will be also decreased The Vex Ultrasonic Kit should be programmed at first in order to use it with robot hardware The required software for programming the Vex kit is EasyC Pro and IFI Loader IFI Loader is available online for free but EasyC Pro asks 100 for installation EasyC Pro is also 24 Page available at WPI ECE Robotics Lab therefore the Vex Ultrasonic Kit is programmed for free at WPI The Vex Ultrasonic Kit with battery Vex Controller Vex Battery and serial interface cable are shown in the image below Figure 10 Ultrasonic rangefinder kit for correcting the heading The code developed to program the Ultrasonic Kit is fairly simple Firstly the library Builtins h that includes Vex robotics commands is added to the directory Then the main loop is written in C programming language There are three basic commands that control the Vex Ultrasonic Kit The whole code is included in Appendix C StartUltrasonic 1 7 start ultrasonic sensor ranging GetUltrasonic 1 7 get ultrasonic sensor r
145. nd Covariance string response Gey lock matlab Put required values for EKF Time Update into MATLAB workspace matlab PutFullMatrix x base State dummy matlab PutFullMatrix P base Covariance dummy2 matlab PutFullMatrix z base WiFiPos dummy3 Run MATLAB Script to do the EKF Time Update Prediction response matlab Execute x P WiFi Measurement Update x Py 2 Get the Updated State and Covariance from MATLAB matlab GetFullMatrix x base ref State ref dummy matlab GetFullMatrix P base ref Covariance dummy2 Create new EKF State EKFState newState new EKFState State Covariance newState Type WiFi Update response Update the EKF s current state currentState State newState State currentState Covariance newState Covariance currentState Type newState Type Save new EKF State ReceivedDat Display SensorUpda a Add newState the updated Received n EKF State and Covariance wState 170 ref Catched MATLAB Exception response catch System Runtime InteropServices COMException comEx Display Error string message Matlab Command scriptName n message Matlab Response response n message Exception comEx Message n MessageBox Show message Console WriteLine EKF WiFi Measurement Response 4 publi
146. nt 0 public override void startUpdating Ery conn Open conn SetMeasurementMode conn InitMT conn SetConfigMode conn SetOutput OrientationMode EulerAngles OutputMode OrientededAndCalibrated conn SetMeasurementMode ui catch Exception ex MessageBox Show Could not connect to IMU ex Message lock matlab matlab Execute clear all close all return base startUpdating Forml mainForm Forml Forml ActiveForm mainForm SafeRenameButton btnStart Stop Updating IMU retur public override void stopUpdating conn Close base stopUpdating Forml mainForm Forml Forml ActiveForm mainForm SafeRenameButton btnStart Start Updating IMU public void ReceivedUpdate object sender ResponseInfo info Extract measured IMU data Array inertial eular if info Type ResponseInfoType EulerAnglesAndCalibrated info Type ResponseInfoType Calibrated inertial new double info accX info accY info accZ info gyrX info gyrY info gyrZ info magX info magY info magZ eular new double info Yaw info Pitch info Roll else 149 Console WriteLine IMU received data that is not Euler Angles and Calibrated return Create IMU data object from Measured Data SensorData newData new IMUData inertial eular Print out Recieved IMU Data Console WriteLine newData ToString
147. ntrolPanel extends JPanel implements FocusListener MouseListener private MoveController moveController private JLabel lblVelocity private JSlider velocitySlider private JLabel controlLabel public ControlPanel MoveController moveCtrl initializeComponents moveController moveCtrl moveController setControlLabel controlLabel Handle displaying key press move controls control state velocitySlider addChangeListener moveController Handle Velocity Changing controlLabel addKeyListener moveController Handle Moving Forward and Backwards and Turning Left and Right using Arrow Keys setVisible true public void initializeComponents this setLayout new GridLayout 4 1 Create Velocity Label lblVelocity new JLabel Velocity Create the velocity slider velocitySlider new JSlider JSlider HORIZONTAL 5 50 30 112 velocitySlider setMajorTickSpacing 10 velocitySlider setMinorTickSpacing 1 velocitySlider setPaintTicks true Hashtable labelTable new Hashtable labelTable put new Integer 5 new JLabel 5 cm s labelTable put new Integer 15 new JLabel 15 cm s abelTable put new Integer 30 new JLabel 30 cm s labelTable put new Integer 50 new JLabel 50 cm s velocitySlider setLabelTable labelTabl velocitySlider setPaintLabels true
148. oString Make sure connected to ER1 Telnet Remote API if lerlController connected return Not Connected to ERI Telnet API Show all recent received data String str ERI Position Wn str erlController CurrentData ToString Mn str IMU Data Mn imuSensor CurrentData ToString UNAG return str 177 WILL OVERRIDE WITH ALGORITHM AND RETURN END RESULT POSITION LER TO USE THEIR FOR NOW FORWARD ALONG TO ER1 CONTROL INTERNAL POSITION ALGORITHM return ER1Controller sendCommand position return UNKOWN REQUEST public bool Start Start the HTTP Listener to receive commands try listener Start catch HttpListenerException ex Handle error starting http server System Windows Forms MessageBox Show ex Message HTTP Server Exception return false try Stop Worker Thread if already running And Create and Start new worker thread if workerThread null amp amp workerThread ThreadState Equals ThreadState Running workerThread Abort workerThread new Thread new ThreadStart HandleRequests workerThread Start catch ThreadStateException ex System Windows Forms MessageBox Show Message x Message Server Start Exception return false return true public void Stop workerThread Abort list
149. ocity if this data Count lt 2 return 0 Get the current and last position to calculate velocity ERIPosition currPos ER1Position this CurrentData ERlPosition lastPos ERlPosition this data this data Count 2 Calculate the distance and time span between positions TimeSpan dt currPos Timestamp TimeOfDay Subtract lastPos Timestamp TimeOfDay double dy currPos YPos lastPos YPos return dy dt TotalSeconds static public bool handlesRequest string request for int i 0 i lt validRequests Length i if request StartsWith validRequests i return true return false override public string ToString string str ERlController Telnet hostname port ToString n str Telnet Status connected Connected NOT CONNECTED Xa str base ToString return str public void Clear base Clear lastCount 0 147 Appendix P 2 3 IMUController cs using System using System Collections Generic using System Ling using System Text using System Windows Forms using ERILocationListener Models using egrath communication XSens using System Threading namespace ERlLocationListener Controllers public delegate void DataReceivedDelegate object sender ResponseInfo info class IMUController SensorController private s
150. oduced better results we implemented that algorithm 0 100 500 600 0 50 100 150 200 250 300 350 400 450 Figure 4 Database for applying the Kernel method algorithm for wi fi position estimate l6lPage For the Kernel method we have to first decide how many locations we want to train our sample data The purpose of this is to generate the database to compare the RSS readings Only with a well trained database will we be able to successfully compute the position estimate As shown from the above figure we selected 12 locations in the route that our robot will navigate across At each of the 12 locations we decided to take 24 training samples The last variable in the Kernel method is the number of access points For the number of APs we chose to use 54 different Mac Addresses This is the total number of Mac addresses that we observed in the entire building With the parameters set for Wi Fi we moved on to analyzing the IMU 2 2 Inertial Measurement Unit for Angle Tracking One hardware device that can identify the change in angle as the robot turns is the Xsens Motion Tracker MTx This motion tracker uses the earth magnetic field to determine the direction of north Using this direction as the reference the MTx can calculate 3D orientations Figure 5 Overview of the inertial measurement unit and its tracking coordinates The MTx sensor measures changes in the x and
151. onarController sonarSensor private CustomAlgorithm positionAlgo private EKFAlgorithm EKFAlgo 179 private WiFiDB wifiDB public Forml InitializeComponent Initialize Parameters int TimeInterval 1000 500 time in milliseconds to wait between sensor data updates string hostname 127 0 0 1 int port 9600 Initialize WiFiDB wifiDB new WiFiDB txtNumLogged this Initialize Sensor Controllers erlController new ER1Controller hostname port 500 btnStartUpdatingER1 get updated ER1Position from ER1 Remote API every 1000 milliseconds imuSensor new IMUController 1000 btnStartUpdatingIMU ref erlController get updated IMUData from IMU every 1000 milliseconds wifiDB imuSensor wifiSensor wifiSensor new WiFiController 5000 btnStartUpdatingWiFi sonarSensor new SonarController 1000 btnStartUpdatingSonar positionAlgo new CustomAlgorithm ref erlController ref TimeInterval btnStartUpdatingPosition EKFAlgo new EKFAlgorithm 100 btnStartUpdatingEKF EKFAlgorithmClass InitEKF Set Handlers for received data erlController SensorUpdateReceived this ER1PositionReceived imuSensor SensorUpdateReceived this IMUUpdateReceived wifiSensor SensorUpdateReceived this WiFiUpdateReceived sonarSensor SensorUpdateReceived this SonarUpdateReceived positionAlgo SensorUpdateReceived this Positio
152. orate these various sensors to take advantage of their individual tracking abilities In the terms of integrating the local sensors we have successfully incorporated the gyroscope and odometer to the fusion algorithm but the ultrasound sensors have yet been integrated The second phase involves developing the application and user interface of our project We developed a user friendly interface that allows for real time remote control map display and algorithm evaluation this allows for real time applications and performance analysis The user interface is written in JAVA while the server used to combine the readings from the sensors is written in C Finally the scripts used to render the readings from each individual sensor are written in MATLAB and C By combining the readings from these various sensors using the Extended Kalman Filter an optimal position estimate can be achieved The position estimates of the local sensors gyroscope odometer and ultrasound sensors are accurate only in the short term because it is subjected to cumulative drift On the other hand Wi Fi localization is accurate in the long term by taking advantage of an existing RF infrastructure but is slow responsive to instantaneous variations However using our sensor fusion algorithm to combine the inertial system and Wi Fi localization the advantages of each can be exploited to achieve optimal results 7 Page CHAPTER I INTRODUCTION Precise localization tec
153. osition in cm centimeters private double yPos ER1 y position in cm centimeters private double angle ER1 angle in degrees always zero private double velocity ER1 velcoity Create ER1Position with x y angle and timestamp given public ER1Position double xPos double yPos double angle DateTim timestamp base timestamp initialize xPos yPos angle Create ER1Position with x y and timestamp given public ER1Position double xPos double yPos DateTime timestamp this xPos yPos 0 timestamp Create ER1Position with x y angle and timestamp of measurement public ER1Position double xPos double yPos double angle base initialize xPos yPos angle Create ER1Position with just x and y coordinate timestamp defaults to current time public ER1Position double xPos double yPos this xPos yPos 0 Create ER1Position with just x and y coordinate timestamp defaults to current time public ER1Position ER1Position pos this pos XPos pos YPos pos Angle pos Timestamp this Velocity pos Velocity Initialize ER1Position with x y position in centimeters and angle in degrees public void initialize double x double y double angle f this xPos x this yPos y this angle angle 128 this velocity 1 public double XPos get return xPos set xPos value public double YPos get return yPos set y
154. ot robot f this robot robot initializeComponents setVisible true public void initializeComponents 109 this setLayout new GridLayout 3 2 lblCommand new JLabel Command txtCommand new JTextField 16 add lblCommand add txtCommand lblResponse new JLabel Response txtResponse new JTextField 16 add lblResponse add txtResponse btnSendCommand new JButton Send Command btnSendCommand addActionListener new ActionListener public void actionPerformed ActionEvent e send command add btnSendCommand public void send command f Make sure robot is connected if robot connected System out println Cannot send command Not Connected return String command txtCommand getText n String response try response robot getClient sendCommand command catch Exception e response NOT CONNECTED txtResponse setText response Appendix 0 3 3 ConnectionPanel java package views import javax swing import java awt event import java awt import models Robot public class ConnectionPanel extends JPanel private Robot robot 110 private JLabel lblStatus private JLabel lblAddress private JLabel lblPort private JTextField txtStatus private JTextField txtAddress private JTextField txtPort private JButton btnConnect private JTextArea txtExtende
155. otView robotView private static final int ZOOM MIN 0 private static final int ZOOM MAX 100 private static final int ZOOM INIT 25 public MapController this mapView null this robotView null public void setMapView MapView mapView this mapView mapView public void setRobotView RobotView robotView 98 this robotView robotView Override public void stateChanged ChangeEvent event JSlider zoomSlider JSlider event getSource if zoomSlider getValuelsAdjusting double zoom zoomSlider getValue 100 0 mapView setZoom zoom robotView setZoom zoom Appendix 0 2 4 MoveController java package controllers import java awt Color import java awt event KeyEvent import java awt event KeyListener import javax swing JLabel import javax swing JSlider import javax swing event ChangeEvent import javax swing event ChangeListener import models public class MoveController implements KeyListener ChangeListener private Robot robot private boolean firstMove true private KeyEvent lastEvent null private int velocity 30 private JLabel controlLabel public MoveController Robot robot this robot robot public void setControlLabel JLabel lblControl controlLabel lblControl Override public void stateChanged ChangeEvent e JSlider velocitySlider JSlider e getSource if velocitySlider
156. path In order to have an idea about the accuracy of the Extended Kalman Filter EKF method the Cumulative Distribution Function CDF can be used In their study Frank Krach Catterall and Robertson plotted the CDF of the Extended Kalman Filter method The CDF method proves that the probability of errors is much smaller when the Extended Kalman Filter method is applied Without EKF the CDF results an average error of 3 2 meters whereas with EKF the average error is reduced to 1 6 meters When there are not too many sample points the Extended Kalman Filter method is a very helpful method to combine the readings from external sensors and to display the real time location of an object 111Page 1 3 Project Description In our project the more general approach the Extended Kalman Filter is studied and implemented to combine readings from Wi Fi Odometer Ultrasound Sensors and IMU A general concept of our robot platform is shown on the following figure Figure 2 General concept of the multi sensor location aware robot with real time remote control and map display As previously mentioned our fusion algorithm will make use of the gyroscope odometer Wi Fi and sonar sensor It uses the angular velocity and wheel encoder for displacement in position and heading and Wi Fi for correcting the drift The sonar sensors have not been implemented but we proposed that the sonar sensor that can used for better angle tracking At the start of the
157. ptPath c ER1LocationListener Matlab Path to Matlab Script Directory private Button btnStart WiFi Controller Button for Starting Stopping Updating private WiFiData wifiData private WiFiDB wifiDB Setup IMU Controller to poll at the specified interval public WiFiController int interval Button btnStartUpdating WiFiDB wifiDB base interval Gey this wifiDB wifiDB btnStart btnStartUpdating Store the IMU Start Updating Button matlab new MLApp MLAppClass Instantiate Matlab Engine matlab Execute cd scriptPath Change Directory to Matlab Script Folder scriptName Get Wi Fi Data Name of Matlab Script to run to get IMU Values catch System Runtime InteropServices COMException comEx System Windows Forms MessageBox Show Message comEx Message nStackTrace comEx StackTrace public override void startUpdating Check connection with ER1 Telnet Remote API if matlab null f string message Cannot connect to Matlab API n string caption WiFi Controller Connection MessageBox Show message caption revue base startUpdating this updating true Forml mainForm Forml Forml ActiveForm mainForm SafeRenameButton btnStart Stop Updating WiFi 153 public override void stopUpdating base stopUpdating Forml mainForm Forml Form ActiveForm mainForm SafeRen
158. r no callback was defined Console WriteLine EKFController data newState SensorDataReceived SensorUpdateReceived n public static void Initialize State 2 WiFiDB wifiDB EKFState newState null lock currentState 165 Magnometer Initalize EKF state and covariance arrays Array state new double EKFState length Array dummy new double state Length Array covariance new double state Length state Length Array dummy2 new double state Length state Length Array sampleMACs new string WiFiDB SamplesRead WiFiDB findMaxAPs Array sampleRSSIs new double WiFiDB SamplesRead WiFiDB findMaxAPs Get Initial State Covariance from Matlab using WiFi and try lock matlab load sampleMACs sampleRSSIs into MATLAB wifiDB save false string response matlab Execute x P Initialize State 2 sampleMACs sampleRSSIs dummy dummy2 matlab GetFullMatrix x base ref state ref matlab GetFullMatrix P base ref covariance ref if response Equals Console WriteLine response catch Exception ex Create new Console WriteLine EKF Init State error ex Message matlab failed so used hard coded values state new double 0 0 O 0 0 0 0 covariance new double 3 3 covariance SetValue 5 0 0 0 x covariance SetValue 5 0
159. rExit btnWZC Text Start WZC private void btnWZCRefresh Click object sender EventArgs e f System Diagnostics ProcessStartInfo psi new System Diagnostics ProcessStartInfo net start psi RedirectStandardOutput true psi UseShellExecute false System Diagnostics Process p System Diagnostics Process Start psi p WaitForExit if p HasExited System IO StreamReader sr p StandardOutput string output sr ReadToEnd if output Contains Wireless Zero Configuration btnWZC Text Stop WZC else btnWZzC Text Start WZC private void btnIMUExportMatlab Click object sender EventArgs e f imuSensor ExportToMatlab private void btnWiFiExportMatlab Click object sender EventArgs e f wifiSensor ExportToMatlab 187 private void btnERlExportMatlab Click object sender EventArgs e erlController ExportToMatlab private void btnSonarExportMatlab Click object sender EventArgs e sonarSensor ExportToMatlab public string GetWIFISignalStrength ERY ManagementClass mc new ManagementClass root WMI MSNDis 80211 BssIdListScan null ManagementObject mo mc CreateInstance mo Active true mo InstanceName Atheros AR5001X Mini PCI Wireless Network Adapter 3 mo UnusedParameter 0 mo Put ObjectQuery query new ObjectQuery SE
160. return false wy return true public boolean updateRobotPosition Robot robot mapPanel getRobot Mak sure we are connected to both the remote location sensor manager server and the ER1 Remote API if this connected return false mapPanel ER1Connected USING OLD T ELNET DIRECT CONN ECTION TO ER1 R EMOT ELN ET API N D a EW API MIMICS OLD SO SAM CODE WORKS 104 and redraw otherwise the robot is connected so send robot command to get position parse update position robot getClient sendCommand move 10 c n robot getClient waitFor move n String positionStr robot getClient sendCommand getEKFPosition customPosition n if positionStr split length 4 System out println Position error positionStr of cm of cm units of degrees return false double x 0 y 0 angle 0 try x Double parseDouble positionStr split 1 in units y Double parseDouble positionStr split 2 in units angle Double parseDouble positionStr split 3 in catch Exception e System out println Position error extracting positionStr robot updateLocation x y angle Position Logging if logger isLogging logger logLocation x y angle return true
161. rn matlab set matlab value public string ScriptName get return scriptName set scriptName value static public bool handlesRequest string request for int i 0 i lt validRequests Length i if request StartsWith validRequests i return true return false override public string ToString string response IMUController Matlab Script waa respons t base ToString return response Appendix P 2 5 SonarController cs using System using System Collections Generic using System Ling using System Text using System Windows Forms using System IO Ports using ERlLocationListener Models namespace ERILocationListener Controllers class SonarController SensorController scriptName private static string validRequests getSonarValues Requests to be handles by Sonar Controller private Button btnStart IMU Controller Button for Starting Stopping Updating private SerialPort sonar 157 Setup Sonar Controller to poll at the specified interval public SonarController int interval Button btnStartUpdating base interval try f btnStart btnStartUpdating Store the Start Updating Button Init Sonar Sensor catch Exception ex System Windows Forms MessageBox Show Message ex Messag nStackTrace ex StackTrace public ov
162. rows 0 Substring timeStart timeLength kj MACList new string rows Length 2 RSSIList new double rows Length 2 for int i 1 i lt rows Length 1 i if rows i StartsWith MAC Console WriteLine WiFiData Parse Invalid wifidata row continue Extract MAC RSSI from row and save into array string MAC rows i Substring rows i IndexOf MAC t 6 i a string RSSI rows i Substring rows i IndexOf RSSI 5 4 MACList SetValue MAC i 1 RSSIList SetValue int Parse RSSI i 1 Return the new wifidata object return new WiFiData MACList RSSIList static public WiFiData Parse2 string data string rows data Split n string MACList new string rows Length 1 double RSSIList new double rows Length 1 for int i 0 i lt rows Length 1 i MACList i rows i Split t 0 RSSIList i double Parse rows i Split Nt 1 134 return new WiFiData MACList public string MACArray get return MACs public double RSSIArray get return RSSIs public object this int index get return new object public int Length get return MACs Length public object this int i TUE J get return j 0 Set 1 if j 0 MACs i else RSSIs i public override string ToString MACs index object MACs i RSSIList RSSIs index object RSSIs
163. rrent values stored for use in later algorithms but it is stored with the past received values so that the algorithms can take advantage of the received set of past values from all the various sensors 40lPage Ser Eoria Se Start Updating All Clear All Paman ER1 Remote API IMU Data WiFi Data Sonar Algorithm EKF Camera EI TI IMU Controller Accel AccelY AcceZ GyX GyY Gyz L Magx MagY MagZ Yaw Pitch Roll Timestamp 0 098229221999 0 6504693031 3 9 806005477905 0 02085560986 0 005463561974 0 01401061285 0 2702516913414 lo 05728345364 0 57809728394 155 93701171875 0 66286319494 4 28240728378 22 47 54 3593750 0 105482667694 072011142969 9 769121170043 004712790995 0007402843330 002304664490 026502719521 009029000314 0 58400940895 155 9941247558 067066750971 4 32207594381 2247543437500 0 076431099149 072773915529 9 7919864654541 001413623243 000541751040 0 028009012341 026380339264 009789522332 059496242751 1556999930964 0 67898225784 4 30675640377 22 47 54 1562500 0 072489887475 0 64305395283 9 795617103576 0102829899148 003380342945 0 01845002546 026418647170 009971020376 05300504554 1155 9097747802 067573356608 4 31611967086 2247539531250 0 076065354049 064662927389 3784482955932 002956027148 000336296390 0 017011275514 027040350437 004985192790 05900749659
164. rror Moving Robot is not connected cobot updateLocation robot getLocation getX 20 robot getLocation getY robot getAngle public void downReleased System out println stop move forward n if robot connected robot getClient sendCommand stopNn true public void leftPressed updateControlLabel true Turning Left System out println turn left n if robot connected f robot getClient sendCommand move 360 d n else System out println Error Moving Robot is not connected robot updateLocation robot getLocation getX robot getLocation getY 20 robot getAngle public void leftReleased f System out println stop turn left if robot connected f robot getClient sendCommand stop public void rightPressed updateControlLabel true Turning Right System out println turn right n 102 if robot connected robot getClient sendCommand move 360 d n else System out println Error Moving Robot is not connected robot updateLocation robot getLocation getX robot getLocation getY 20 robot getAngle public void rightReleased System out println stop turn right n if robot connected robot getClient sendCommand stopNn public void updateControlLabel boolean inControl if inControl updateControlLabel
165. s data Split if parts Length 4 data Contains front data Contains back data Contains left data Contains right Console WriteLine SonarData Error parsing sonar data data return SensorData NoSensorData 137 return new SonarData parts 0 Trim parts 1 Trim parts 2 Trim parts 3 Trim Appendix P 2 Robot Server Controllers Appendix P 2 1 SensorController cs using System using System Collections using System Ling using System Text using System Timers using ERlLocationListener Models namespace ERlLocationListener abstract class SensorController public delegate void SensorUpdateReceievedDelegate SensorData data private static string validRequests list of requests that this Sensor Controller handles private int interval Polling interval for sensor in milliseconds protected ArrayList data Array of received values protected bool updating Will poll sensor at interval when updating is true protected Timer timer Timer used for executing doUpdate ever interval protected SensorUpdateReceievedDelegate didUpdate public SensorUpdateReceievedDelegate SensorUpdateReceived return didUpdate didUpdate value ge se public SensorController this interval 1000 set the timer interval to 1 second updating false initializ
166. s endp startp end for a 1 diff pointstl x step x diff diff points y step y diff diff points x ground startp 1ta x ground startp 2ta tx step y ground startp 1ta y ground startp 2ta ty step end end 86 Appendix M Get IMU Data MATLAB Codes oe Get Real Time Data m This function reads one sample of data from the IMU Modified Version of MT Easy Script oe oe function inertialData Get IMU Data if exist h 0 3 Creates the server h actxserver MotionTracker CMT 3 Create a CMT instance serial 43206 40120 9E20D B4A57 h cmtCreateInstance serial 3 Set Parameters COMport 6 baudrate 115200 3 Opens port for IMU h cmtOpenPort COMport baudrate 3 Checks to see if MTx is connected num MTs h cmtGetMtCount if num MTs 0 error MTx not connected end request device information from MotionTracker deviceld h cmtGetMtDeviceId 0 dec2hex deviceId 3 Put MotionTracker in config mode h cmtGotoConfig heading 0 location 0 samplefreq 100 3 request calibrated inertial and magnetic data along with orientation data h cmtSetDeviceMode 6 5 samplefreg deviceld 3 Orientation outputmode sample counterteuler 100 Hz 3 That s it 3 CMT is ready to start processing the data stream from the MotionTracker h cmtGotoMeasurement 3 start processing data end 3 wait short moment for object to read data from COM port pause 0 1
167. s Using the prediction and measurement the filter can then update the Kalman gain and the covariance The state transition matrix is not updated in the regular Kalman filter however it is updated in the Extended Kalman Filter The set of equations used to compute the prediction phase are shown below Equation 10 Prediction Equations of Kalman Filter In the above equations the predicted state x and covariance P are computed using the previous input and covariance Additionally this computation requires the use of the state transition matrix A and the system noise Q In general the input controls vector u and the input matrix B is set to zero as noted by 11 The next step is to calculate the measurement update and modifying the Kalman gain to better equate the measurement with the prediction a I K P H HP H R 5 K 2 HA gt 2v Il P I K H P Equation 11 Measurement Update Equations of Kalman Filter First the Kalman gain is computed using the observation matrix H the predicted covariance P and the measurement noise R In general H is default as the identity matrix Using the Kalman gain measurementz predicted state and the observation matrix the next Welch and Bishop Page 5 10 Welch and Bishop Page 5 76 equation will then update the state Lastly the filter updates the covariance It is also important to note that the covariance is fed back into the predic
168. search concludes that the only type of sensor that can fit into the project s budget is ultrasonic sensor Comparing the two different ultrasonic sensors above it is observed that Maxbotix has a larger range and lower accuracy and price Further research at WPI resulted that the Vex Ultrasonic Kit is available for free at Robotics Department Therefore the sensor that is tested at first is Vex Ultrasonic Kit 68 A More Different Approach Line Tracker Vex Company has another sensor called the line tracker This sensor has a very simple working structure similar to the ultrasound sensor It basically has an infrared sensor on it which scans the colors of the surfaces that it is facing The line tracker sensor is able to detect wide margins of change in colors For instance if we draw a yellow line on a dark surface the line tracker sensor makes it possible for the robot to follow that yellow line therefore to go straight Since drifting is the major problem we have with the movement of ER 1 robot we can use this line tracker sensor to make the robot follow a pre defined path The problem with the usage of line tracker sensor is its incompatibility with the user interface One of the goals of our project is to control the robot through remote wireless connection without seeing it In this case if the robot follows a pre defined path our application loses one if its main features Therefore we should come up with alternative methods to imple
169. sound sensor is fairly simple in this sense There might be several methods to implement the usage of ultrasound sensor in our system depending on the place where the sensor is mounted or the number of sensors The first method that we tried was mounting the sensor to the front side of the robot such that it detects the closest object in the robot s path OUTPUT Figure 40 Vex Ultrasound Sensor mounted to four different sides on the robot After implementing and taking measurements about the first method it turned out that mounting the sensor to the front side of the robot might not be the best method to make use of that First of all the ultrasound sensor has a limited range and its readings cannot be perfectly accurate due to the way that sound waves travel along their path The robot that we are using ER 1 has a built in obstacle detector which is easy to program and implement Therefore using the new ultrasound sensor for just obstacle detection would not make sense 73 An alternative way of implementing the usage of ultrasound sensors in our system is to place four of them in different places around the robot We assume we place four ultrasound sensors to the front left right and back sides of the robot respectively With this method we will be detecting the closest objects to the front and rear sides of the robot The sensors on the left and right sides of the robot will be measuring the distance of the robot to the walls in th
170. t sensor combinations and algorithms The second feature of our Major Qualifying Project is the statistical Wi Fi localization algorithm This algorithm makes use of the existing Wi Fi infrastructure inside a building which makes it an inexpensive sensor with low power consumption The algorithm we used does not require a lot of data training We went to twelve different locations and logged twenty four samples in each location These twenty four samples are logged in four different directions Furthermore by using an existing infrastructure and developing a software based position tracking engine the performance of the algorithm can be easily accessed and evaluated The algorithm itself can be easily modified to further enhance the performance The last feature is that we designed and implemented the Extended Kalman Filter as the core fusion algorithm that takes advantage of the short term tracking ability of our local sensors such as the Odometer and Gyroscope and the long term tracking ability of our global sensors such as Wi Fi By using our global sensor Wi Fi RSS readings we can successfully correct the drift that is imposed by the inertial sensors Gyroscope and Odometer Without the reference 56 Page aided by Wi Fi the inertial algorithm dead reckoning achieved an error rate greater than 20 meters On the other hand Wi Fi alone achieved ninety percent of the error to be within 4 5 meters of its actual position The discrepancy can b
171. tY this angle angle else Update x y coordinates in cm and anglein degrees location setX x location setY y this angle angle Record updated location robotTrace addPoint location Appendix O 1 2 Map java package models import java util ArrayList import java beans XMLEncoder import java beans XMLDecoder import java io FileOutputStream import java io FileInputStream public class Map private String name private double width private double height ArrayList Line lines private final double PixelsPerCM 0 25 private final double CMPerFoot 30 48 91 public Map name width 800 height 600 lines new ArrayList Line loadTestMap public void loadTestMap set canvis size this width 62 CMPerFoot this height 80 CMPerFoot outer map addLinelInFeet 1 1 61 1 addLineInFeet 1 79 61 79 addLinelInFeet 1 1 1 79 addLineInFeet 61 1 61 79 addLineInFeet 7 7 55 7 addLineInFeet 7 73 55 73 addLineInFeet 7 7 7 73 addLineInFeet 55 7 55 73 public Map Map map this name map name this width map width this height map height this lines ArrayList lt Line gt map lines clone public double getWidth return width public double getHeight return height public String getName return name public void setName String
172. taking the measurements of many points inside of a building and by using the RSS of the access points AP it estimates which point in the building the object is closest to simply by calculating the distance as shown in 1 The nearest neighbor method is efficient only if a large amount of data samples are taken Hence using the nearest neighbor method is sometimes unpractical 1 The second method Kernel method is based on the idea of using a probability mass function and the Gaussian curve to estimate the position of the object From past results the Kernel method has proven to be more accurate and more practical than the nearest neighbor method Hence our Wi Fi localization technique will be largely based on the Kernel method I5 Page that is described in 6 The Kernel method focuses on estimating the final location based on the probability of each location After careful consideration of both methods we decided to take the Kernel method approach From the results in 6 the approximate error of the Kernel method is about 5 meters This is a significant error however it can be corrected with the aid from other sensors and perhaps a filter as well The nearest neighbor method on the other hand yielded an error of roughly 5 5 to 6 meters Also we will be ignoring the Histogram method that was tested in 6 Hence in comparison to the nearest neighbor method the Kernel method proves to be far more superior Since the Kernel method pr
173. tatic string validRequests getIMUValues Requests to be handles by IMU Controller private MLApp MLAppClass matlab Create MATLAB object to run Seripts private string scriptName Matlab Script to Execute to Read Values from IMU private string scriptPath c NNERlLocationListenerNMMatlabNN Path to Matlab Script Directory private Button btnStart IMU Controller Button for Starting Stopping Updating private static bool simulation false private Connection conn private int lastCount index of the last IMU data used private ER1Controller eriController Setup IMU Controller to poll at the specified interval public IMUController int interval Button btnStartUpdatingIMU ref ER1Controller erlController base interval this erlController erlController TEY btnStart btnStartUpdatingIMU Store the IMU Start Updating Button matlab new MLApp MLAppClass Instantiate Matlab Engine matlab Execute cd scriptPath Change Directory to Matlab Script Folder lastCount 0 conn new Connection COM6 Connection DataReceivedEvent ru new Connection DataReceivedEvent ReceivedUpdate conn DataReceived ru catch System Runtime InteropServices COMException comEx 148 System Windows Forms MessageBox Show Message comEx Message nStackTrace comEx StackTrace public void ClearCount f lastCou
174. tion phase to update the predicted covariance Below are the results of a Kalman filter e Voltage Figure 42 Kalman Filter Results The cross marks represent the noisy measurements the straight line is the true voltage value and the curve is the Kalman Filter output As shown above the Kalman filter was able to adapt the noisy measurements to approximate the true value By choosing a constant for the prediction noise Q and the measurement noise R the filter will update very quickly the covariance P and the Kalman gain K as noted by 11 Since this is a linear filter as previously mentioned it does not account for non linear conditions Hence it may seem more practical to use its evolved model the Extended Kalman Filter Nevertheless we may be able to make use of this filter on the Wi Fi localization measurements since it is a smoothing filter after all Welch and Bishop Page 13 TI Appendix F Wi Fi Kalman Filter Results In the previous Wi Fi section we have shown that Wi Fi localization have a large amount of instantaneous movement This refers to the parts when Wi Fi immediately jumps from one location to the other Hence by adding the Kalman Filter to Wi Fi the data show a much smoother state transition 60 80 100 120 140 60 180 200 220 240 260 Figure 43 Raw Wi Fi Red vs KF Wi Fi Black Results This first plot illustrates the tracking of the regular Kalman Filter of Wi Fi versus the Raw W
175. uble ToMatlabArray return new double AccelX AccelY AccelZ GyrX GyrY GyrZ MagX MagY MagZ Yaw Pitch Roll Timestamp TimeOfDay TotalMilliseconds Appendix P 1 5 WiFiData cs using System using System Collections using System Ling using System Text namespace ER1LocationListener Models class WiFiData SensorData private string MACs private double RSSIs public WiFiData Array MACList Array RSSIList base this MACs new string MACList Length for int i 0 i lt MACs Length i MACs i string MACList GetValue 1 this RSSIs new double RSSIList Length for int i 0 i lt RSSIs Length i RSSIs i double RSSIList GetValue i public WiFiData Array MACList Array RSSIList DateTime timestamp base timestamp 133 this MACs new string MACList Length for int i 0 i lt MACs Length i MACs i string MACList GetValue 1 this RSSIs new double RSSIList Length for int i 0 i lt RSSIs Length i RSSIs i double RSSIList GetValue i public static WiFiData Parse string data Create array to store all MAC and RSSI data Array MACList RSSIList Split the wifiscan string into rows for parsing MAC RSSI data string rows data Split n Extract the timestamp int timeStart rows 0 IndexOf TIME 6 int timeLength rows 0 IndexOf r timeStart string time
176. ur JAVA interface 37 Page lolo pa ui ERA Nadigatog ILL T Connection Command Control Logging v Detail View Meading 90 0 NP Figure 18 Client interface with the video stream on the upper right control system on the lower right and the map location display on the left As shown in the figure above our current JAVA interface has a velocity meter a command line a video stream and a log file Furthermore we also added zooming capabilities to the map of the building 4 3 Robot Location Sensor Server The Robot Server Software component is responsible for polling all the various sensors at their independent intervals as well as logging the received data points for later use in algorithms and analysis using MATLAB 38 IPage Remote Communtcaon lt TS Figure 19 Robot server side of the architecture responsible for sampling the different sensors and applying the fusion algorithm on the sensors The Robot Server exposes an HTTP Web Interface so the client can get the current position controlling the robot s movement and receive a real time video feed The ER1 Locator Server is responsible for handling requests from the client for a position polling sensors and triggering MATLAB to process the algorithms inputs and outputs Currently the ERI Remote Control Panel is used by connecting to its Remote API via the TELNET protocol and can be used to send commands directly to t
177. ven with large errors during some time instances it is still able to recover the position eventually as long as the robot continues its route In order to better view the errors we performed one of the most famous graphical analyses in the realm of statistics the Cumulative Distribution Function This plot illustrates the amount of error that we get and the frequency of each of them 49 Page CDF Plot of Wi Fi vs Ground Truth Probability 0 1 2 3 4 5 6 7 8 Error in Meters m Figure 28 Cumulative density function of the wi fi localization algorithm The CDF analysis is one of the most used functions in analyzing the results of certain tests This is primarily because it shows the probability of something happening In our case 90 of our position estimations portray an error that is less than 5 meters Most indoor localization errors that range from 2 to 3 meters are generally sought after However as shown in 1 typical Wi Fi localization errors range from 3 to 6 meters depending on the selected parameters Thus with respect to other Wi Fi localization data our results are fairly good One possible way to reduce the error is to use a filter that incorporates another device with the received signal strength measurements A filter that we paid most of our attention to is the Kalman Filter and the Extended Kalman Filter Both filters are ideal for state transitions However this is a great start to using Wi Fi localization to find
178. ving Forward System out println move forward if robot connected f String recMsg String doneString boolean moveDone false if firstMove f try Thread sleep 100 catch InterruptedException e TODO Auto generated catch block e printStackTrace System out println Checking if Move Done recMsg robot getClient sendCommand events n doneString move done moveDone recMsg indexOf doneString gt 0 if moveDone firstMove if firstMove System out println First Move sending command if moveDone System out println Last Move Done but move key still pressed resending move command robot getClient sendCommand move 50000 cm n firstMove false else System out println Error Moving Robot is not connected cobot updateLocation robot getLocation getX 20 robot getLocation getY robot getAngle public void upReleased System out println stop move forward n if robot connected robot getClient sendCommand stop n else System out println UpReleased STOP MOVNG FAILED firstMove true public void downPressed 101 updateControlLabel true Moving Backwards System out println move backword if robot connected f if firstMove f robot getClient sendCommand move 500000 cm n firstMove false else System out println E
179. ween 600 and 700 seconds the errors are all less than 4 meters Furthermore immediately after the large error the algorithm was able to correctly locate the robot s position With the timing analysis we generated a CDF of the results to better understand the error The following figure illustrates our resulting CDF 52 Page CDF Plot of EKF vs Ground Truth Probability 0 1 2 3 4 5 6 rf 8 Error in Meters m Figure 31 Cumulative density function of the Extended Kalman Filter It can be clearly seen from our above figure that 90 of the error is less than 1 meter At that mark the error is estimated to be 2 7 meters In terms of performance in indoor environment our results are very good As mentioned before typically the error in most indoor localization is around 3 meters Nevertheless we have tweaked the parameters of our EKF as well as the database in our Wi Fi localization to the fullest given our time constraint Lastly the EKF can not only fix the position errors but also minimizes the drift of the ERI and IMU data It combines the continuous tracking ability of the dead reckoning algorithm with the Wi Fi Kernel algorithm to output the most accurate position estimate We acknowledge that further improvement can be done with the parameters of the EKF but our results are certainly large contributions to indoor localization 53 Page 5 5 Comparative Performance Analysis The purpose of this section is to compare the per
180. xPos int xPos PixelsToCM yPos int yPos PixelsToCM dataGridViewl Rows Add new object xPos yPos DateTime Now TimeOfDay private void btnClearLog Click object sender EventArgs e dataGridViewl Rows Clear 88 Appendix 0 1 Robot Remote Control Client ER1Navigator Models Appendix 0 1 1 Robot java package models java awt Graphics java awt Color import import import import public REMOTE controllers LocationHardwareController controllers LocationSensorManagerServer class Robot private Point initialOffset private Point location private double angle private double width private double height private ER1Client robotClient USES OLD ER1 REMOTE API use ER1 Location Sensor Manager Server private LocationSensorManagerServer robotClient private boolean connected private RobotTrace robotTrace private final float CMPerFoot 30 48f public Robot API start robot 5 feet from origin in x and y direction location new Point int 4 CMPerFoot int 4 CMPerFoot angle 0 initialOffset new Point 0 0 initialOffset new Point 28 CMPerFoot 70 CMPerFoot initialOffset new Point 5 CMPerFoot 5 CMPerFoot set the width height to around 8 inches 20cm width 35 height 35 Create new robot trace to track path of robot robotTrace new
181. y direction horizontally and the z direction vertically Furthermore the MTx can also compute the 3D acceleration 3D rate of turn 3D earth magnetic data In order to make these measurements the MTx uses a gyroscope Magnetic Field Mapper MT Manager Add on User Manual Page 2 3 MTi and MTx User Manual and Technical Documentation Pages 1 and 8 17 Page accelerometer and a magnetometer The gyroscope is used for measuring the orientation the accelerometer for measuring the acceleration and the magnetometer for measuring the magnetic field Furthermore all the data read by the MTx can also be logged The following figure displays the types of data that the MTx sensor logs MTData DATA pao at a2 a MID 50 0x32 DATA P i 4 Figure 6 Inertial measurement unit data log format From the above figure it is clear that the MTx sensor measures acceleration orientation and the magnetic field in the X Y and Z directions with an accuracy of 4 bytes However all three measurement devices are prone to drift One of the challenges of using is this MTx sensor is to determine the behavior of the drift and develop a method to compensate for the drift The two components of the IMU that we will take advantage of are the angular velocity readings from the gyroscope and the heading direction from the magnetometer 2 2 1 Gyroscope for Sensing Angular Velocity From the past work we believe that the gyroscope readings wou
182. y if the covariance becomes too high If the covariance is not high then the Wi Fi measurements will not have much of an impact on the position coordinates With the development of the EKF equations measurements and state complete we have to determine the covariance values 1 cm 0 0 Q 0 1 cm 0 Eq 9 0 0 0 527 35 cm 0 R Eq 10 0 35 cm a With every time update the covariance term P will be updated with the noise from the matrix Q Once the noise from the x and y position reaches a certain point then the Wi Fi measurement update will have a larger impact The results of our EKF algorithm will be further discussed in Chapter V The upcoming section is the Software Design and Integration 3A Page CHAPTER IV SOFTWARE DESIGN AND INTEGRATION This project is composed of four parts The sensor part consists of various sensors which take readings from different devices The controller is responsible for picking the right data to use Algorithm block applies different algorithm to determine the position of the algorithm The exact position is displayed on the user s interface 4 1 Overview of Project Architecture The Architecture of this Robotic Indoor Localization System consists of two parts the robot server which is implemented in C for sensor data acquisition and remote communication The client can connect control and monitor with the robot server through its HTTP Web Server Video Stream Location 4 Map D
183. z catch System Runtime InteropServices COMException comEx Catched MATLAB Exception Display Error string message Error exporting WiFi Data to Matlab n IF message Matlab Command scriptName An message Exception comEx Message Mn MessageBox Show message static public bool handlesRequest string request for int i 0 i lt validRequests Length i if request StartsWith validRequests i return true return false override public string ToString string response SonarController samples data Count Nts respons t base ToString return response 160 Appendix P 2 6 WiFiDB cs System Collections Generic ER1lLocationListener Models using System using System Collections using using System Ling using System Text using using System Windows Forms namespace ERILocationListener Controllers class WiFiDB public delegate void WiFiSamplesReceievedDelegate WiFiDB data private WiFiSamplesReceievedDelegate gotAllWiFiSamples private string name private ArrayList samples private MLApp MLAppClass matlab private int samplesToRead private int samplesRead private TextBox txtTotalSamplesRead private Forml mainForm public WiFiSamplesReceievedDelegate onAllWiFiSamplesReceived get return gotAllWiFiSamples set
Download Pdf Manuals
Related Search
Related Contents
Betriebsanleitung Feinwaage PBS/PBJ Capacitación Hoja de registro Cliente: Kintetsu World Express (KWE Co-pilot - Permobil Sirene de Exterior OS Calibreur de laboratoire VLB avec dispositif de secouage Manual de Usuario TV 1000 TDT 3,5” Ikelite L1 User's Manual OWL Intuition Room Sensor 8100 X-max 0W-40 Copyright © All rights reserved.
Failed to retrieve file