technical documentation - linköping university · 2016-01-22 · the mine sweeping functionality...

Autonomous Mine SweeperLiTH

2015-12-07

Technical DocumentationEditor: Oscar Hörberg

Version 1.0

StatusReviewed 2015-12-07Approved Hanna Nyqvist 2015-xx-xx

TSRT10Oscar Hörberg

Ross [email protected]

LIPs


2015-12-07

PROJECT IDENTITY2015/HT, Ross Haj

Linköping University, Dept. of Electrical Engineering (ISY)

Group membersName Responsibility Phone EmailAxel Halldin Project Leader (PL) 070-3035052 [email protected] Hörberg Head of Documentation (DOC) 076-1308547 [email protected] Griph Head of Design 070-9413964 [email protected] Svensk Head of Software 070-6343977 [email protected] Örn Head of Hardware 070-3724874 [email protected] Fåhraeus Head of Tests 073-7270416 [email protected] Hyllengren Head of Sensor Fusion 073-0858131 [email protected]

Email list for the whole group: [email protected] site: http://www.isy.liu.se/edu/projekt/tsrt10/2015/bandvagn/

Customer: Saab Bofors Dynamics, LinköpingCustomer contact: Torbjörn Crona, [email protected]

Course leader: Daniel Axehill, 013-284042, [email protected]: Hanna Nyqvist, 013-281 353, [email protected]

Tutors: Martin Lindfors, 013-281365, [email protected]örn Johansson, [email protected]

Carl Nordheim, [email protected]



LIPsPage I


2015-12-07

Contents

Document history IV

1 Introduction 1

1.1 Project history . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

2 System Overview 2

2.1 System flow . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.1 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.2 Sensor Diagnosis and Signal Processing (SDSP) . . . . . . . . . . . . 3

2.1.3 Positioning and Mapping . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.1.4 Base Station . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.1.5 Hand Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3 Base station 6

3.1 Main GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

3.1.1 Sensor status . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.1.2 Control of Balrog . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.1.3 Obstacle Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3.1.4 Current location of Balrog . . . . . . . . . . . . . . . . . . . . . . . . 8

3.2 Load predefined Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3.3 Sensor Plot GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.3.1 Sensor Plots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.3.2 Sensor Toggle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.4 Communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

4 Hand controller 11

5 Balrog 12

5.1 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

5.1.1 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

5.1.2 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

5.1.3 Odometers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

5.1.4 Ultrasonic sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

5.1.5 Arduino . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

5.1.6 Implementation and flow in subsystem . . . . . . . . . . . . . . . . . . 20



LIPsPage II


2015-12-075.1.7 Dependencies to other systems . . . . . . . . . . . . . . . . . . . . . . 20

5.2 Hardware problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

5.2.1 The LIDAR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

5.2.2 The ARM processor . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

5.2.2.1 Recommendation for future developers . . . . . . . . . . . . 22

5.3 Sensor Diagnosis and Signal Processing (SDSP) . . . . . . . . . . . . . . . . . 23

5.3.1 Sensor modelling and signal pre-processing . . . . . . . . . . . . . . . 23

5.3.2 Sensor Diagnosis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

5.3.2.1 Outlier rejection . . . . . . . . . . . . . . . . . . . . . . . . 27

5.3.2.2 Sensor Reliability . . . . . . . . . . . . . . . . . . . . . . . 28

5.3.3 Data available from SDSP . . . . . . . . . . . . . . . . . . . . . . . . 28


5.4 Positioning and Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

5.4.1 Pre-filter Processing . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

5.4.2 Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

5.4.2.1 Motion Model . . . . . . . . . . . . . . . . . . . . . . . . . 32

5.4.2.2 Measurement Model . . . . . . . . . . . . . . . . . . . . . . 33


5.4.4 Dependencies to other subsystems . . . . . . . . . . . . . . . . . . . . 35

5.5 Implementation Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

5.5.1 Shared data structure . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

5.5.2 Subsystems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

A Appendix: Sensor Characteristics 38

B Appendix: Message Id’s 40

References 42



LIPsPage III


2015-12-07Document history

Version Date Changes Sign Reviewed0.1 2015-12-10 First draft all all0.2 2015-12-14 Second draft all all



LIPsPage IV


2015-12-07

1 Introduction

The Autonomous Mine Sweeper project is an ongoing project with the long term goal to con-struct a prototype of an autonomous mine sweeping crawler. The project is a collaborationbetween Saab Bofors Dynamics and Linköping University. The history of the project is de-scribed in the subsection below.

The purpose of this document is to give a detailed description of the subsystems that have beenmodified and developed during this year’s project. For a thorough description of previously im-plemented subsystems, see the previous years’ "Technical Documentation" [7]. The documentshall give the client and customer a thorough description of the system. It shall also give anoverview of what future projects need to do and what they can focus on for the project to reachits long term goal.

1.1 Project history

The history below is composed from earlier documentation and this year’s experiences.

The project was started in the spring of 2009 with the student group O’hara’s. O’hara’s mainfocus were specification, control programs, network communication and navigation techniques

In autumn 2009 the group Carpe Locus continued where O’hara’s had left and developed remotecontrol of the crawler and automatic control for the propulsion. The crawler was also equippedwith GPS.

The next autumn (2010) the group 8Yare continued by mounting an industrial computer on thecrawler and ported the old code to the new computer. The crawler was also mounted with stereocameras (model Bumblebee 2).

In 2011 group iMAP focused on using the camera and the crawlers sensors to create a 3D mapof the operating area.

In 2012 Minenmarker choose not to continue the work with the 3D map. Instead they developedthe mine sweeping functionality and improved the crawlers positioning. The main goal of theproject in 2012 was to verify that the entire area had been searched and that all the mines werefound. Minenmarker also named the robot Balrog.

In 2013 the group Ostende Abscondita continued to improve the positioning and search algo-rithm. They also integrated a wireless hand controller for manual control of Balrog.

In 2014, the project group Invenire Periculosa’s main goals were to continue to develop Bal-rog’s positioning. Balrog was equipped with a 360°laser sensor and a more powerful industrialcomputer.

This year, 2015, the main goal has been to improve the positioning of Balrog. To make this taskeasier a better IMU-unit, also including a barometer, has been mounted on Balrog. Furthermorea few problems in the filter and base station from the last years has been solved. A whole newsubsystem has been implemented and is named Sensor Diagnosis and Signal Processing orshort SDSP, details in Section 5.3. The motion model in the filter has been expanded and newsensor models has been added to improve the performance of the filter.



LIPsPage 1


2015-12-07

2 System Overview

The mine sweeping system is divided in three main subsystems, the Base Station, the HandController and Balrog. Balrog is a small crawler that consists of different subsystems. Eachsubsystem performs specific tasks and includes hardware, software or both. For a schematicoverview of the system see figure 1. The arrows show the communication flow in the system.

Balrog

Exte

rnal

Com

mun

icat

ion

API

Hand Controller

Base Station

Wireless link

SensorsIMU

Odometer

Laser scanner

GPS

Ultrasonic

Sensor Diagnosis and Signal Processing

Positioning and MappingMine detection

Signal pre-processingPropulsion

Electric Motor

Control

Route planning

Description

Hardware

Software

Component

ARM processor

GUI

Wireless link

Information Flow

Barometer (in IMU)

Obstacle detection

Sensor diagnosis

Navigation filter

Map/Mapping

Figure 1: Illustration of the system and its subsystems. The communication flow in the systemis illustrated by arrows.

This year’s project has focused on the subsystems "Sensors", "Sensor Diagnosis and SignalProcessing" and "Positioning and Mapping". The design goal was to keep as much as possiblefrom last year, and therefore the other subsystems have not been changed unless it was necessaryfor the system to work well. The Base Station was modified to meet the requirements in the"Requirement Specification" [5] and some more work was made to get a working system.



LIPsPage 2


2015-12-072.1 System flow

In this section the system flow will be described and the focus will be according to the designgoal mentioned above. In other words, the data that is passed between the subsystems RoutePlanning, Control and Propulsion or between them and other subsystems has been kept thesame as in last year’s project if possible. Therefore, only the subsystems where the data flowhas been changed considerably are described below. To understand the flow between the othersubsystems, see the "Technical Documentation" from last year [7].

2.1.1 Sensors

In the Sensors subsystem raw sensor data is stored and time stamped. This data is then avail-able for the Sensor Diagnosis and Signal Processing subsystem. Most of this was already im-plemented from last year, but the code that log IMU data was modified to fit the new IMUsensor that also includes a barometer. The raw sensor data is sent to the Base Station for debugpurposes.

2.1.2 Sensor Diagnosis and Signal Processing (SDSP)

This subsystem receives raw and timestamped sensor data and pre-processes it. Its purpose isto provide the navigation filter with high quality data. The system runs marginally faster thanthe filter, meaning fresh data is always available for the filter. Each processed sensor sample isstored together with the time stamp from the corresponding raw sensor data, a flag that tells ifthe sensor is reliable/unreliable, a flag that marks if the data is an outlier or not and a flag thattells if the data has been read by the navigation filter in a data structure, see figure 2. The datastructure of pre-processed sensor data is used by the Positioning and Mapping subsystem andsent to the Base Station.

From Positioning and Mapping the SDSP subsystem receives information needed in the sensordiagnosis. The SDSP subsystem can also receive commands from the Base Station telling theSDSP subsystem to treat a sensor as reliable/unreliable.

Processed sample

Flag: reliable/unreliableTimestamp Flag:

outlier/inlierSensor 1

Processed sample

Flag: reliable/unreliableTimestamp Flag:

outlier/inlierSensor N

Flag:read/unread

Flag:read/unread

Figure 2: Schematic of a package of processed sensor data written by SDSP. Packages is storedin a shared data structure. The filter uses the latest package as input.



LIPsPage 3


2015-12-072.1.3 Positioning and Mapping

The Positioning and Mapping subsystem use the pre-processed data from the Sensor Diagnosisand Signal Processing subsystem as input to the navigation filter, obstacle detection, mine de-tection and mapping (if there is no predefined map). In this year’s project the mine detectionhas not been covered and no changes has been made to that algorithm.

This subsystem is in charge of all maps that are used in the mine sweeping system. Only thepredefined obstacle map was of intereset this year and is therefore the only map existing onBalrog. This map is shared between the basestation and Positioning and Mapping.

2.1.4 Base Station

The Base Station and Balrog have been modified so that the Base Station can receive the fol-lowing data from Balrog:

• Raw sensor data with time stamp (from Sensors)

• Pre-processed sensor data with time stamp and flag (from Sensor Diagnosis and SignalProcessing)

• Estimated states (from Positioning and Mapping)

• Maps: obstacle map

The Base Station and Balrog have been modified so that the Base Station can send the followingdata to Balrog:

• Control commands to make it move (to Control)

• Set sensors unreliable/reliable (to Sensor Diagnosis and Signal Processing)

• Predefined obstacle map (to Positioning and Mapping)

All data sent between the base station and Balrog will be encapsulated as shown in Table 1according to existing implementation [7, sect. 4.1]. There are some new messages being passedbetween the base station and Balrog and the id of those messages can be seen in Table 2 andTable 3.

The payload of the new message sent for controlling sensor toggle on Balrog con-sists of an array with one element for each sensor. Each element has one ofthree values, SENSOR_TOGGLE_ALWAYS_USE, SENSOR_TOGGLE_AUTOMATIC_USE orSENSOR_TOGGLE_NEVER_USE defined in the new data class SensorToggle.

2.1.5 Hand Controller

The hand controller is the first way of controlling Balrog in manual mode. Further details onhow it communicates and what commands that are available are explained in Section 4.



LIPsPage 4


2015-12-07

ID Payload length Payloaduint16_t id uint16_t len char payload[len]

Table 1: Data format for a message between Balrog and the base station

ID DescriptionNETWORK_MESSAGE_<sensor>_SAMPLE_SDSP

Sends the buffer of processed sen-sor data from SDSP periodically.One message for each sensor, <sen-sor> is replaced with the name ofthe sensor, for exact naming of eachmessages se Appendix B.

NETWORK_MESSAGE_OBSTACLE_MAP Sends the obstacle map as an arrayfrom Balrog periodically.

Table 2: New messages sent from Balrog to base station

ID DescriptionNETWORK_MESSAGE_OBSTACLE_MAP Sends the predifinied map structure

as an array, see [7] for more details.NETWORK_MESSAGE_LINE_MAP Sends the predifined map as all the

lines in the map, coordinates forstart- and end-point.

NETWORK_MESSAGE_SENSOR_TOGGLE Sends information whether the sen-sors should always be used, neverused or automatic used

Table 3: New messages sent from the base station to Balrog



LIPsPage 5


2015-12-07

3 Base station

The base station is a laptop communicating with Balrog via WiFi. The purpose of the basestation is to make it possible for the operator to control, tune, debug and see the activity onBalrog. To do this the base station application uses a GUI and it is the functionality of this GUIthat is explained in this section.

To display activity on Balrog things as sensor values, calculated position, speed of the motorsare represented to the operator in different ways. Some more detail about different functions ofthe basestation can be found below.

In this chapter some internal data classes are mentioned, if extensive explanation of how eachdata class works is left out further description can be found in Technical Documentation 2014,[7].

The communication between the base station and Balrog connected to each feature in the GUIwill be explained in more detail in Section 3.4.

3.1 Main GUI

For the most part the main GUI has been kept as it was in the beginning of this year’s project.The minor changes are a new element in the status bar and two new options in the file menu(input map and plot gui). Details about the changes are described in the subsections below. Thenew design of the main GUI can be seen in Figure 3.

Functionallity in the main GUI in use and working are:

• Display sensor status

• Control by arrow keys

• Set top speed, both keys (’w’,’s’) and vertical slider (visual in GUI) can be used

• See location of Balrog

• See the full obstacle map

Due to a problem with the propulsion system it isn’t possible to control Balrog in practise rightnow, for more detail about this problem read Section 5.2.2. As soon as that is resolved it ispossible to operate Balrog via the base station.

Functionality in the GUI not mentioned above isn’t implemented on Balrog and neither are themessages needed in the communication for said feature, for example waypoint handling andautomatic search.

For more detail about each part see respective subsection below.



LIPsPage 6


2015-12-07

Figure 3: Current layout of the main GUI

3.1.1 Sensor status

As mentioned above the status bar of the old GUI has been expanded with one new feature. Itis now possible to see the status of each sensor on Balrog.

A sensor can be flagged as Empty, reliable or unreliable. If a sensor is flagged as Empty theprocessed data array connected to that particular sensor is empty on the base station. This occurswhen Balrog hasn’t collected enough samples from the sensor so that the SDSP subsystem canstart filling the array with processed data. When a sensor is flagged reliable one of two thingshas happened. Either the operator controlling Balrog has requested that the sensor values alwaysshall be used from the GUI or the SDSP has analysed the raw data from the sensor and flaggedit as reliable. The implementation is the same if a sensor is flagged as unreliable.

More details about the decision whether a sensor is unreliable or reliable can be found in Sec-tion 5.3.2 and more about manual sensor toggle can be found in Section 3.3.2.

3.1.2 Control of Balrog

With a working propulsion system the Balrog can be controlled by the arrow keys on the basestation. It works by key press-events that triggers a function in the base station application.The function sends the requested motor speeds to Balrog and the ARM communicator threadon Balrog then makes the tracks move. For future development this chain will probably changeas the propulsion system needs a rework due to malfunctioning ARM processor. If the sameconcept of sending motor speeds to Balrog is kept then it shouldn’t be needed to change theway a key press is handled on the base station.



LIPsPage 7


2015-12-07

Figure 4: Structure of the file containing the predefined map

3.1.3 Obstacle Map

To display the current environment in which Balrog operates the base station the data classObstacleMap is used. How the class works is described in Technical Documentation 2014,[7]. The three different states of a grid in the obstacle map is drawn as; UNEXPLORED_TILEand EMPTY_TILE is painted white and OBSTACLE_TILE painted black.

The line representation in the predefined map will be translated to an obstacle map containinggrids with a, by the user, chosen resolution. More information about that in Section 3.2 below.

3.1.4 Current location of Balrog

The location of Balrog can be seen in the same section of the GUI as the obstacle map. Thelocation is drawn by using the state data sent from the filter subsystem on Balrog and the uncer-tainty of this position, also contained in the state data, is drawn as an ellipse around the locationof Balrog.

3.2 Load predefined Map

There is a new feature in the file menu of the main GUI for loading a predefined map. Thepredefined map needs to be defined as seen in Figure 4 for the parser creating the internal mapto work.

As of now the application creates two representations of the map and sends them to Balrog. Bothan ObstacleMap and a DataCollection object containing all the lines in the map. Thereason to create both is that different parts of the system is interested in different representationsof the map. It also makes it possible to use either representation in future implementations onthe system.

These internal maps are meant to be used by the filter and controller in Balrog when a prede-fined map is available. The controller subsystem was not considered this year so there is noimplementation using the available map for route planning and similar algorithms. The useof the predefined map in the filter is partly implemented but not all the way. Since problemsoccurred late in the project and focus shifted to simulations the last part of this implementa-tion was left out. The thing left to do is to implement the read of the predefined map in thePositioningAndMapping subsystem. The filter will then use the map loaded on to Bal-rog.

The feature of loading a predefined map is based on two classes, MapParserGUIand MapParser. The MapParserGUI class is called from a UserInterface object inthe bases station application and in turn the MapParserGUI calls the MapParser doing the



LIPsPage 8


2015-12-07

Figure 5: Design of the new sensor plot GUI.

actual parsing. This way it is possible to implement other ways of reading a predefined mapand the only thing that needs to be changed is the parser function in the class MapParser.

The standard file that defines the map is located in SOURCE_DIR/basestation/map.txtbut it is possible to input any absolute path to a file where a map is defined according to thestructure in Figure 4.

3.3 Sensor Plot GUI

The sensor plot GUI is new for this year and responsible for displaying all sensor values to theoperator in real time. This part of the GUI is also responsible for manual sensor toggle. Thedesign of the sensor plot part of the GUI can be seen in Figure 5. Each part of the plot GUI isdescribed in more detail below.

3.3.1 Sensor Plots

For each sensor both raw and processed data is plotted in the same plot. Raw sensor data as ablue line and processed sensor data as a green line.

It is possible to choose which component (i.e x,y,z) of each the sensor that should be displayedby pressing the corresponding radio button. All content of the current plot will then be updated.At the moment just one component can be displayed at a time.

3.3.2 Sensor Toggle

To change the status of a sensor the operator presses one of the radio buttons in the bottompart of the sensor plot GUI. When the operator presses the button a message is sent to Balrogcontaining all the current values of each sensor toggle and actions are taken accordingly in theSDSP subsystem on Balrog.



LIPsPage 9


2015-12-073.4 Communication

The basics of the communication between Balrog and the base staion are explained in TechnicalDocumentation 2014, [7]. The different messages passed between the two systems can be seenin Table 4.

The Balrog side of the communication uses message ids connected to the data classes imple-mented and corresponding communication layer to send and receive messages.

The base station on the other hand uses a combination of data class communication layersconnected to message ids and its own communication class NetworkBaseStation to sendand receive messages. Which method chosen on the base station side depends on what type ofmessage that is sent.

This year’s project group has tried to go towards the Balrog sides way of handling messages onthe base station as well and it should be the long term goal to do this system wide. The reasonis that it becomes a uniform way of message handling and almost no duplicated code is needed.

The main parts of implementing new messages to be sent between the systems are now:

• Naming the new message and giving it an id in MessageIds.

• Creating a Comlayer on Balrog if it doesn’t already exists.

• Making sure the Balorg send the new message in it’s communication thread.

• Adding a case for parsing the new message in the receive on the base station

• Adding a function for sending the new message from the base station to Balrog.

• Connecting the event in the GUI that should send the new message.

In some way the steps for implementing a new message above also represent how a message ishandled on the current system. This process becomes some what time consuming and the stepscould be cut in half if both systems would be based on the same ground of communication.There is a need for at total rework of the communication part of the GUI both a structural andfunctional.



LIPsPage 10


2015-12-07ID DescriptionNETWORK_MESSAGE_OBSTACLE_MAP Sends the predefined map structureNETWORK_MESSAGE_SENSOR_TOGGLE Sends information whether the sen-

sors should always be used, neverused or automatic used

NETWORK_MESSAGE_<sensor>_SDSP Sends the buffer of processed sen-sor data from SDSP periodically.One message for each sensor, <sen-sor> is replaced by the name of thesensor. See Appendix B for exactnaming.

NETWORK_MESSAGE_<sensor> Sends the buffer of sensor datafrom the sensor samplers periodi-cally. One message for each sensor,<sensor> is replaced by the name ofthe sensor. See Appendix B for ex-act naming.

NETWORK_MESSAGE_STATE_DATA Sends the current state data fromthe filter.

NETWORK_MESSAGE_MOTOR_SPEED Sends motorspeeds between thebasestation and Balrog, both leftand right.

NETWORK_MESSAGE_LINE_MAP Sends a line representation of thepredefined map to Balrog.

Table 4: Messages sent between the basestation and Balrog, a combination of Table 2 and 3along with alredy implemented messages.

4 Hand controller

The hand controller is used to operate Balrog in manual mode. The controller is an Xbox 360controller communicating over the 2.4 GHz band with a dongle connected via USB to Balrog.

The HandController class handles the communication with the hand controller. It followsthe same structure as the other runable classes. When input from the controller is read usingthe C libraries linux/input.h and linux/joystick.h the appropriate action is taken.Currently nothing else than driving Balrog is implemented, meaning that the only thing theHandController class modifies outside its own private members is the MotorSpeed datastructure, which later is sent to Balrog to determine the speeds of the tracks.

For information about the buttons and their functions, check this year’s User Manual [8]



LIPsPage 11


2015-12-07

5 Balrog

This section will describe the subsystems on which this year’s project has been focused on,namely Sensors, Sensor Diagnosis and Signal Processing (SDSP) and Positioning and Mapping.

5.1 Sensors

Balrog is equipped with a number of sensors, which are used to collect data about Balrog andits environment. All sensors but the odometers and the ultrasonic range sensors are directlyconnected to the main processing unit. The ultrasonic sensors are connected via an Arduino,which handles sampling and settings. The odometers are connected to an ARM processor.However, this processor stopped working in the final week of this project, see Section 5.2.2 formore information.

This section will focus on explaining the new sensors for this year. See previous year’s DesignSpecification and Technical Specification for more details on previously mounted sensors [6,sec. 5.2, p. 215], [7]. Note that the laser sensor has been removed from previous year, since itwas broken. Also, the IMU has been replaced with a new one this year.

In Table 5 all sensors mounted on Balrog can be found.

Sensor Measured physi-cal quantity

Output Running in Standardrange

Gyroscopes Angle velocity,heading

deg/s 10 Hz 0-450 deg/s

Accelerometers Velocity, acceler-ation

m/s2 10 Hz 0-50 m/s2

Magnetometer Heading, minedetection

a u (arbitraryunits, normalizedto earth fieldstrength)

10 Hz 0-80 µT

Barometer Altitude Pa 10 Hz 300-1100hPa

Ultrasonic sen-sors

Distance (to ob-ject)

cm 10 Hz 0-6 m (11m max)

GPS Position WGS84 position,number of satel-lites used, accu-racy

1 Hz -

Odometer Distance (trav-elled), speed

mm, mm/s Event based (Notrunning. ARMbroken.)

1.07mm(maxresolution)

Table 5: Sensors that are mounted on Balrog



LIPsPage 12


2015-12-075.1.1 GPS

The GPS installed on Balrog is of the type Ublox-5. It is connected to one of the USB ports,and has a separate antenna to receive the satellite signals.

The sensor output values according to the WGS84 standard. However, these coordinates areconverted to the RT90 standard in the software and logged to be used by other subsystems.

Sensor interface

All communication with the sensor is handled via USB. The power needed by the sensor is alsodrawn from the USB port.

Communication

The communication with the GPS is one way; Balrog never sends any messages to the GPSsensor. The GPS continuously sends samples in a stream. To retrieve them, the port the GPS isconnected to is opened and read.

Placement on Balrog

The GPS sensor is mounted inside Balrog (in the compartment under the main processing unit).The antenna is mounted on top of Balrog’s metal plate in order to get as good reception aspossible.

Sensor model and performance

After testing the GPS, it was clear that the noise in the samples was not of White Gaussian type.For a further explanation about how this was handled and modelled see Section 5.3.1

5.1.2 IMU

An IMU-unit, which also includes a barometer and a magnetometer, is mounted on Balrog. Thesensor is of type MTi 100 and is manufactured by Xsens. This is an upgrade from last year’sBalrog. The MTi 100 contains the following sensors:

1. 3D accelerometer

2. 3D gyroscope

3. 3D magnetometer

4. Barometer

5. Temperature sensor

All facts about the sensor in this section are taken from The MTi User Manual [2], unless statedotherwise.

See Figure 6 for a picture of the sensor.



LIPsPage 13


2015-12-07Sensor interface

The IMU is connected to the main processing unit via USB. The USB cable also provides powerto the IMU.

Communication

An SDK package is provided by Xsens. It contains the Xsens Device API which was used forcommunication with the MTi 100. The API is written in C and compatible with Windows andLinux systems, but the provided C++ wrapper has been used in this project. Important to noteis that the API works for all of Xsens’ sensors and not just MTi 100. This means some of thefunctions in the API will not provide a meaningful return value, because they target a featurethat does not exist in the MTi 100.

Xsens also offers a complete specification of the low level communication [3].

On board processing

The MTi 100 has an on board processor which handles sampling of the sensors and has builtin signal processing software. The unit contains an internal 16 bit ADC to convert the analogsensor values to digital. There is an option to get the raw, unprocessed (but digitalized) sensorvalues as output. See the Output section for specifications of what outputs were used.

Placement on Balrog

The IMU is mounted so its center is on Balrog’s Z-axis, in order to minimize transient acceler-ations while rotating around the Z-axis.

The sensor is mounted upside down inside Balrog. The IMU and Balrog’s X-axis corresponds,while there is a sign change in Y and Z-axis. This is compensated for in software, so the loggedvalues of the IMU corresponds to Balrog’s coordinate system. See Figure 6 for the IMU’scoordinate system and Figure 7 for Balrog’s.

Coordinate system

The sensor fixed coordinate system is a right handed system, see Figure 6.

Used outputs

The inertial outputs used from the MTi 100 are preprocessed 3D linear acceleration, 3D rate ofturn (gyroscope) and 3D magnetic field data. All values are in a sensor-fixed coordinate sys-tem. The calibrated data has been going through Strapdown Integration and Inverse StrapdownIntegration. The output from the barometer was also used. Table 6 shows which outputs wereused and their corresponding unit.



LIPsPage 14


2015-12-07

X

Z

Y

Figure 6: The MTi 100 and its default coordinate system

Vector UnitAcceleration m/s2

Angular velocity (rate of turn) rad/sMagnetic field a.u.(arbitrary units, normal-

ized to earth field strength)Pressure Pa

Table 6: Used output from the MTi-100

API

The API for MTi-100 differs somewhat from the other sensor APIs. Unlike the other APIs itis implemented in two blocks. The first block handles initializations. The second block is acallback handler which handles the sampling of the sensor.

Application flow

The use of the IMU API differs a little compared to the other sensors’. Programatically, it doesnot inherit from the same abstract base class (ASampler), this is due to the use of an eventbased callback handler.

First, the initialization of the sensor is run (connection to the device, setting frequency andwhich outputs are required from the sensor). In this step, a callback handler is added to thesensor. Every time the sensor sends data, the callback handler is triggered. The callback handlerthen checks what kind of data is received, timestamps it and logs it. See Figure 8 for a schematicof the communication flow.


After stationary testing of the MTi-100 it was clear that all sensor noise could be considered asWhite Guassian Noise with the variances specified in Appendix A. See Section 5.3.1 for furtherdetails and sensor model.



LIPsPage 15


2015-12-07

X

Y Z

Front

Back

Figure 7: Balrog’s coordinate system, seen from above

MTi-100

User

Data log

Hardware

Software

Globally accessible data structure

On board processing

IMU API

Callback handlerInit Software

subsystem

Figure 8: Flow for the MTi-100, the arrows shows the direction of communication

5.1.3 Odometers

The odometers measure how much each track on Balrog moves, using 500 marks spaced 1.0744mm. These sensors are event based and sends out pulses on two lines each time a mark is passed.Which pulse comes first determines the direction. From this information it is also possible tocompute the speed of the tracks. For a more thorough explanation, see the design specificationof 2009 [4].

Since the ARM-processor that previously handled the sampling of the odometers has stoppedworking, there is currently nothing that samples the odometers. See Section 5.2.2 for hintsabout how to implement the ARM functionalities in the Arduino.



LIPsPage 16


2015-12-07Sensor interface

The communication with the odometers needs to be handled by some kind of microcontroller,which then in turn has an interface towards the main processing unit of Balrog.

Currently, the ARM processor which previously was used for this purpose is broken, see Section5.2.2.

Communication

Two wires are used for the communication with each odometer. When one of the evenly spacedmarks on the plate, which is mechanically coupled to the tracks, passes the sensor two pulsesare sent, one on each wire. In which order they come determines which direction the plate isturning. For more details, see Section 5.2.2.1

Placement on Balrog

The odometers are mounted inside Balrog, next to the motors.


The sensor has event based sampling and measurements were taken while rotating Balrog onthe spot in order to determine the variance of the sensor. The results can be found in Table 7below. For further details and sensor model see Section 5.3.1

Left track Right trackVariance ∆distance [mm] 3.6916 · 10−6 1.0776 · 10−6

Variance speed [mm/s] 2.1725 · 10−4 2.6475 · 10−4

Table 7: Variances of odometer samples

5.1.4 Ultrasonic sensors

Balrog is equipped with four ultrasonic sensors to measure distances and detect objects. Theyare of type SRF10 and the specifications for the sensors can be found in [9]. The sensors got abeam width of 72° [10].

Sensor interface

The ultrasonic sensors are connected to and controlled by a microcontroller, an Arduino UnoRev.3, through an I2C (Inter-Integrated Circuit) bus. The Arduino is, thanks to the I2C bus, ableto communicate to multiple sensors on the same physical wire.



LIPsPage 17


2015-12-07Communication

The Arduino uses Arduino’s Wire Library to communicate with the I2C devices [11]. Eachsensor has a unique address that specifies which sensor is to be read or written to at the moment,see table 8. The SRF10 sensors uses eight bit addresses but the I2C protocol just handles sevenbits for the address when sending (the eight bit that is sent through I2C determines if the deviceis being written to or read from). Therefore the lowest bit is manually dropped in the Arduinocode and the seven most significant bits are sent to the sensor where it reads as an eight bitaddress. For example, for a sensor whose address in the data sheet is specified to 0xE0, 0x70 issent instead.

Direction Position 7-bit Address 7-bit Adr (Hex) 8-bit Adr (Hex)

ForwardLeft

Right115117

0x730x75

0xE60xEA

Left 119 0x77 0xEERight 121 0x79 0xF2

Table 8: I2C address for each ultrasonic sensor. The sensors got a 8-bit address but it is the7-bit address which is sent through the I2C bus.

Placement on balrog

The sensors are mounted on Balrog as shown in figure 9 and the table 9 below. There aretwo sensors pointing forwards and two sensors pointing sideways at each front corner, makingBalrog able to detect objects on its sides. The front sensors are angled 10 degrees each fromeach other to reduce the risk that one "ping" will be detected in wrong sensor.

β

Figure 9: Layout of the positions of the ultrasonic sensors

Arduino - ultrasonic circuit

The sensors are connected in parallel as they are connected to the same I2C bus. A 470µFcapacitor in parallel with the range finders is used to smooth their power supply. Two set ofresistors, with a total resistance of 1.79kΩ each, are connected from the Arduino’s 5V pin tothe SDA (serial clock) and SCL (serial clock) pins. This is to pull those lines high.



LIPsPage 18


2015-12-07Sensor(address)

Position x(cm)

Position y(cm)

Angle againstthe −y-axis(degrees)

115 +22 +18 +10117 +22 −18 −10119 +14 +22 +90121 +14 −22 −90

Table 9: Positions of the ultrasonic sensors. The positions are counted from the origo at Balrogin Balrog’s coordinate system.

Sensor operation

The ultrasonic sensors have four different internal registers which can be written to and readfrom via the I2C bus. These are the command register, analogue gain register and range register(which actually occupies two registers). A range measurement is started by sending a commandto the command register of a sensor and the result can be read from the range register when themeasurement is finished. By sending different commands the user can determine if the resultshall be given in inches, centimetres, or in micro-seconds. The sensors are currently set to givethe result in centimetres.

The maximum range can be set by writing to the range register. It determines the internaltimer, which says how long the sensor will listen for an echo. With a shorter ranging time theultrasonic sensor can make more measurements during the same time interval. The maximumrange is currently set to 280 cm.

The maximum analogue gain can be set by writing to the analogue gain register. A lowervalue decreases the maximum gain of the "ping" from the sensor and the echoes from objectsfurther away will be more difficult to detect. This is good to do if the sensor has to make fastrange measurements without picking up a distant echo returning from the previous "ping". Themaximum analogue gain is currently set to 0x06.

Both the maximum range and the maximum analogue gain are set at the Arduino start up.

For further details, see the data sheet for the SRF10 sensors [9].


After stationary testing at four distances against a wall with each of the SRF10 sensors, itappeared that the noise could be approximated as white Gaussian noise for each distance butalso that the variance was increasing with the distance. Cause of this the biggest variance wasused for all sensors to make the choice simple. The variance was set to 0.073. See Section 5.3.1for further details.

The ultrasonic sensor with 7-bit address 0x73 has approximately a linearly increasing error asthe range measurement increases. This error is about one centimetre less than the real rangevalue for each half meter and an offset is implemented for this in the Arduino code.

The Arduino does the range measurements for the sensors simultaneously, which means thatthere is a risk that one sensor picks up the other’s echo and get a fake measurement. However,this has been tested and did not shown any significant disturbance and should not be a problemsuch as the sensors are mounted on Balrog.



LIPsPage 19


2015-12-075.1.5 Arduino

The Arduino unit is an Arduino Uno Rev. 3 and is an open-source microcontroller board basedon the ATmega328P with and pre-installed bootloader, [12]. To write the code and program theArduino unit, Arduino’s own software was used.

Arduino interface

The Arduino is connected to Balrog’s main processing unit via USB. The USB cable also pro-vides power to the Arduino.

Communication

The Arduino starts the ranging loop right after start up. After it gets the values from the sensorsit creates a string of the values. The string, which have a pre defined structure that the ArduinoAPI in Balrog’s main processing unit also know, is sent via the USB to the API. When theAPI listens to the stream it gets the latest ranging values and a time stamp is set to the data.Currently, the Arduino makes range measurement and sends data more often than the API islistening at the steam which means some data will get lost.

5.1.6 Implementation and flow in subsystem

Each sensor subsystem includes the following:

1. The hardware (including eventual on board processing provided by the manufacturer)

2. An API which handles communication with the sensor

3. A log where all sensor data is stored

The task for the API is to handle all interaction with the sensor. This includes fetching sensordata. The API can also set change internal sensor settings. All measurements that are fetched bythe API are timestamped and stored in a shared data structure available to the other subsystemswithin Balrog.

See Figure 10 for a schematic of the flow.

5.1.7 Dependencies to other systems

Each sensor subsystem is independent. Thus every sensor subsystem can be run in its ownthread without any considerations.

5.2 Hardware problems

Unfortunately some hardware started to malfunction during the project and some was brokenfrom start. In this section the problems are explained, what was done to resolve them andrecommendations about what to do in the future.



LIPsPage 20


2015-12-07

Sensor

API

User

Data log

Hardware

Software

Globally accessible data structure

On board processing

Figure 10: Flow of a sensor subsystem. The arrows shows the communication.

5.2.1 The LIDAR

Previously a 360 rotating laser scanner (LIDAR) was mounted on top of Balrog to retrievedistance measurements to objects surrounding Balrog. This sensor did not work the first time itwas tested.

The communication with the sensor works fine. A health status byte can be retrieved fromit, and it shows no signs of errors. However, sensor samples cannot be retrieved using themanufacturers API. When sensor samples for a full revolution (this is the only way the APIenables the user to access samples) are requested a timeout error code is returned. This isprobably due to one of two factors:

• The laser sampler is broken, and thus no samples can be retrieved

• The angle measurement sensor is broken, and thus the sensor never gets that it has com-pleted a full revolution

If there is an interest in determining the exact source of the problem, this can most likely bedone using low level communication.

5.2.2 The ARM processor

The ARM processor, which was used for controlling the tracks and sampling the odometers,suddenly stopped communicating after a package intended to simplify the overloading of codefrom PC to Balrog was installed. Most likely some update of Ubuntu on Balrog occurred, whichchanged some kind of communication setting. It is unknown which package update caused theproblems and what the update changed.

The ARM works without trouble on Windows system, and also on a computer with Ubuntu12.04. However, on all computers (4 different tested in total) with the latest update of Ubuntu14.04, the communication is dead.



LIPsPage 21


2015-12-075.2.2.1 Recommendation for future developers

Since the ARM is currently not working, Balrog cannot be driven and no odometer samples canbe retrieved. This year’s recommendation to future developers is to scrap the ARM processorand implement the same functionality in the Arduino, which currently only is used for samplingof the ultrasonic range sensors. There are a few reasons for why this option is better thancontinuing to try to get the ARM to work again:

• When reading on forums it is evident that other people have had a lot of problems withthis particular ARM processor and Linux; the compatibility with Linux seems to be bad

• The ARM processor is old and outdated. So old that not even the technicians at ISY hadany hardware to program it. Working with such hardware is not safe, in case it needs tobe reflashed in the future.

The current functionality implemented in the ARM-processor is:

• Sampling of the odometers and by request sending the current speed and the travelleddistance since last request

• A PID-controller which takes a speed for each track as input and makes sure this speed isheld

• Communication with the main processing unit via USB, using a keyword based commu-nication protocol

This, or similar, functionality should be implemented in the Arduino. Currently the ARM-processor is not removed from Balrog, but mounted and connected as it was when it was work-ing. For further information about how the propulsion and odometers work, consult the designspecification of 2009 [4].



LIPsPage 22


2015-12-075.3 Sensor Diagnosis and Signal Processing (SDSP)

The SDSP subsystem has be added by this year’s project in order to perform sensor modellingand sensor diagnosis. The sections below describe how the SDSP subsystem works.

5.3.1 Sensor modelling and signal pre-processing

The purpose of the sensor modelling and signal pre-processing is to process the sensor dataand model the noise. If the noise is non-white Gaussian, an inverse filter has been modelled inorder to get a signal with white Gaussian noise (WGN). The navigation filter in Positioning andMapping will work better if the noise is WGN.

Typical for WGN is that the Power Spectral Density (PSD) is constant, the Auto-CorrelationFunction (ACF) is a scaled unit impulse and that the Probability Density Function (PDF) is aGaussian distribution.

The sensors are considered to act according to the following model:

y = x+ e (1)

Where y is the measurement, x the quantity that should be measured and e is the measurementnoise (e ∼ N(0, σ2)).

Noise modelling

To get a noise model that describes reality as well as possible, multiple data sets have beencollected. The measurements were made during a few minutes (1-5 minutes).

The data sets from each sensor were examined using periodograms and autocorrelation func-tions, in order to see if the noise could be considered as white or non-white. To be able totell if the noise was Gaussian, different plot tools in MATLAB was used (histogram, ndist andnormplot). The variances of the WGN was estimated to get a good starting point for tuning thenavigation filter. The estimation was made by calculating the variance of the noise of each dataset and then taking the mean of all results. The noise variances can be found in Appendix A. Allsensors in the new IMU was considered to have WGN. One example is shown in figure 11 andfigure 12, where it is possible to see that the measurement noise of the accelerometer could beapproximated as WGN. The measurement noise of the odometer was also approximated to beWGN.

The ultrasonic sensors were also considered to have WGN. This was not really obvious butbecome clearer for longer distance measurements. The calculated variances for the ultrasonicsensors were, as mentioned earlier in Section 5.1.4, increasing with the distance. The variancesbetween the sensors did not differ as much as they were differing between a short range mea-surement and a longer one. Because of this and to keep it simple, one and same value was usedfor all four ultrasonic sensors. This value was the biggest of the calculated variances for allsensors and distances. Using the largest variance was considered to be reasonable because theperformance of the sensor is then not overestimated. The ultrasonic sensor tests were performedin microseconds to obtain higher resolution as if it has been in centimeter instead. The valuefor the calculated variance was afterwards converted for fitting range measurements taken incentimetre.



LIPsPage 23


2015-12-07

(a) Autocorrelation function y coordinate, ac-celerometer

(b) Periodogram y coordinate, accelerometer

Figure 11: The accelerometer has approximately white noise

If the noise was non-white Gaussian, an inverse filter was found by system identification. Thiswas made by examining the noise using linear models in MATLAB’s System IdentificationToolbox [14]. For this modelling the data sets were divided into model data and validation data.Of all sensors mounted on Balrog it was only the GPS that was considered to have non-whiteGaussian noise. A first order AR-model was considered to be good enough for inverse filteringthe signal. The coefficients can bee seen in Appendix A. The model had the following structure:

ewhite(n) = enon−white(n) + a1 ∗ enon−white(n− 1) (2)

Figure 13 shows the ACF of the noise of the y coordinate before and after inverse filtering andfigure 14 shows raw and filtered measurement noise compared to each other.

The data sets were also analyzed in order to find an eventual bias in the sensor. It was onlypossible to know if there was a bias for the sensors that measure something with a knownground truth, for example a distance sensor. The sensor data were compensated for the biasif it was considered necessary. Depending on how the sensor worked, the bias was estimatedoff-line or on-line. If the bias seemed to be constant both when Balrog was moving and whenit was stationary, the bias was set off-line. Otherwise it was set on-line. Because the IMUwas mounted a bit obliquely an on-line bias compensation was made to get no accelerationin x and y when Balrog was standing still on flat ground. For the other sensors in the IMUand the odometer no bias compensation was made. For the ultrasonic sensor at address 115,there was an approximately linearly increasing bias for increasing distance measurements. Thebias was not depending of first range measurements values and was, as mentioned in Section5.1.4, off-line bias compensated in the Arduino code. For the other ultrasonic sensor no biascompensations was needed.

Because of major difficulties in determining an exact input signal, which would be the groundtruth of what the sensor is measuring, especially for a dynamic input signal, the sensor mod-elling was made during a constant-but-unknown-input experiment (the ground truth was notknown). Therefore, the models do not describe the dynamics of the sensor. It will only describethe stationary properties.



LIPsPage 24


2015-12-07

Figure 12: A histogram of accelerometer measurement noise compared with a normal distribu-tion

In cases where measurable noise contributors (disturbances) were able to be identified, the noiseshould have been modelled based on these contributors. One example of such a contributoris the electric propulsion motors that are likely to create disturbances in the magnetometermeasurements. Unfortunately, the ARM processor stopped working (and thus the propulsion)and it was not possible to get a measurement with a noise contributor for the magnetometer.Therefore, the magnetometer was not modelled the way it should have been. On the other hand,it was modelled when Balrog was standing still and no electric motors were on. As mentionedabove the noise could be estimated as WGN and the variances is shown in Appendix A.



LIPsPage 25


2015-12-07

(a) Before inverse filtering (b) After inverse filtering

Figure 13: Comparison between GPS measurement noise before and after inverse filtering(through a first order AR-model)

(a) y plotted against x (b) y coordinate plotted against samples

Figure 14: Raw GPS measurement noise compared with inverse filtered noise



LIPsPage 26


2015-12-075.3.2 Sensor Diagnosis

The sensor diagnosis part of the SDSP subsystem has two major parts, sample outlier rejectionand detecting when a sensor is reliable/unreliable. The SDSP uses the result from the outlierrejection to determine whether a sensor is reliable; if a reliable sensor receives enough consecu-tive outliers, the sensor is considered unreliable; likewise, if a unreliable sensor receives enoughconsecutive non-outliers, the sensor is considered reliable.

The sensor diagnosis part of the SDSP subsystem does not alter the sample values, instead it’sadding flags to data samples so the filter will know the status on both the sensor and the sampleitself.

5.3.2.1 Outlier rejection

The outlier rejection is made in different ways for different sensors, depending on whether thesensor value is represented in the Position and Mapping Measurement Model. For all sensorsbelow, the sensor sample value is denoted ysensor.

Note: These tests are only implemented on working sensor’s SDSP. However, the outlierrejection code can be more or less copy-pasted from other sensor’s SDSP for a fast imple-mentation.

Measurement model sensors

For sensors represented in the measurement model, the outlier rejection is based on a designparameter (threshold) σ for each sensor and the probability density function p(yk+1|y1:k) foreach sensor.

The filter used in Positioning and Mapping is a marginalized particle filter (see section 5.4).Each particle given by that filter will contain the nonlinear states xn,i, the linear states xl,i, thecovariance P l,i and the weight wi. This information together with the general model structureof the marginalized particle filter [15] gives that a measurement y is distributed as a Gaussianmixture:

y ∼ p(yk+1|y1:k) ≈N∑i=1

wik+1|kN (hk(xn,ik+1|k)+Hk(xn,ik+1|k)xl,ik+1|k, Hk(xn,ik+1|k)P l,i

k+1|kHk(xn,ik+1|k)T+R)

(3)Where R is the covariance of the measurement noise.

The filter provides the SDSP with y in (3), as well as y’s variance V . The sample is consideredas an non-outlier if

|ysensor − y| <√V ·NUMBER_OF_SIGMAS = σ ·NUMBER_OF_SIGMAS (4)

where σ =√V and NUMBER_OF_SIGMAS correspond to a confidence interval. The

default NUMBER_OF_SIGMAS is 2, which correspond to a 95.45 % confidence interval.By changing the value of NUMBER_OF_SIGMAS, it will be possible to tune the test. Allsensor values that do not fullfill (4) will be treated as outliers.

Note: NUMBER_OF_SIGMAS is not tuned on Balrog.



LIPsPage 27


2015-12-07Non-Measurement model sensors

For remaining active sensors, a measurement estimate yest. is extrapolated from previous valuesusing a Lagrange polynomial of order N . Using a limit, L, a sensor value is considered as anon-outlier if

|ysensor − yest.| < L (5)

By varying N and L, (5) can be tuned individually for each sensor.

Note: The test is not tuned for any sensor.

5.3.2.2 Sensor Reliability

The sensor reliability test is the same test for all sensors. If a reliable sensor receives K con-secutive outlier samples, the sensor is flagged as unreliable. If a unreliable sensor receives Kconsecutive non-outlier samples, the sensor is flagged as reliable. K can be changed to tune thetest.

Note: Due to abstraction, K will be the same for each sensor’s SDSP. To change this,overrule variables in inherited SDSP classes.

5.3.3 Data available from SDSP

The processed data available from the SDSP subsystem will have the same physical quantityas given by the sensor converted to SI units. The purpose for this is mainly to make the filtermore modular. The only exception is the GPS, which converts from the sensor’s WGS84 format(spherical coordinates) to RT90 (Cartesian coordinates with unit meters) as well as local coord-nates which is a translated RT90 format. Ergo, the GPS is converted to a different standardwhich uses SI units.

5.3.4 Implementation and flow in subsystem

The SDSP subsystem includes the following:

1. Filtering of non-Gaussian noise

2. Sensor diagnosis

3. Data from sensor subsystem

4. Data to filter

5. Data from filter

The SDSP filter the incoming GPS signal with it’s inverse noise model in order to get a morewhite Gaussian noise. The GPS SDSP also creates a local coordinate and attaches this to thesample. Neither the unit conversion nor the filtering is needed for remaining sensors because thesensor subsystem delivers SI units and the sensor noise can be approximated as white during



LIPsPage 28


2015-12-0710 minute runs. The processed data is then used by the sensor diagnosis to perform outlierrejection and sensor reliability tests. The SDSP also adds a read/unread flag to the samples inorder to give the filter an easy way to check if there’s fresh data in the DataCollection.

The SDSP for each sensor is run on a separate thread, inheriting from the ARunnable class.All SDSP classes also inherits from the abstract class ASDSP which contains it’s work thread,SDSP flags, variables and virtual methods for the SDSP functionality. These virtual methodsshould be overridden when a sensor’s SDSP needs to modify data or flags.

When the SDSP has processed and diagnosed the data, the data is stored in aDataCollection object of type Processed<Sensor>Sample (insert sensor namein <Sensor>). This data object inherits from the Sensor subsystem’s <Sensor>Sampleas well as the abstract class AProcessedSample. AProcessedSample contains theflag functionality while the <Sensor>Sample contains the sensor value with it’s orig-inal data types. The consequence of this is that data from <Sensor>Sample andProcessed<Sensor>Sample can be accessed through the same variables and data typesand that the flags from all Processed<Sensor>Samples can be accessed through the samevariables across all sensors.

The base station has the possibility to set the reliable/unreliable-flag for any sensor to a fixedstate through public methods. This means that the sensor diagnosis will have the permission toupdate the flag. The flag can also be unset so that the sensor diagnosis will be able to updatethe flag. This will be implemented through a private flag which will or will not give the sensordiagnosis updating authority.

See Figure 15 for a schematic of the flow. Note that all sensors are not used on Balrog at themoment, these have no Sensor Diagnosis box, because outlier rejection is not performed.

Dependencies to other systems

The different inverse noise filtering, the outlier rejection and the sensor diagnosis can be exe-cuted independently from each other and can be run on the same thread as its correspondingsensor. This means that every sensor filtering and diagnosis will independently fetch the latestfilter output available.



LIPsPage 29


2015-12-07

Ultrasonic sensor data

Accelerometer data

Barometer data

GPS Data

Gyroscope Data

Magnetometer data

Inverse noise model filtering

Sensor diagnosis

Sensor diagnosis

Sensor diagnosis

Sensor diagnosis

Data from filter

OdometerData

Processed Ultrasonic Sensor Data

Processed Odometer Data

Processed Accelerometer Data

Processed Barometer Data

Processed GPS Data

Processed Gyroscope Data

Processed Magnetometer Data

Convert to local coordinate

Figure 15: Flow of the SDSP subsystem. The arrows shows the communication.



LIPsPage 30


2015-12-075.4 Positioning and Mapping

The purpose of the positioning and mapping subsystem is to estimate Balrog’s position, orien-tation, velocity and angle velocity. A sketch of the positioning and mapping system can be seenin Figure 16. The positioning and mapping subsystem was called the SLAM subsystem in the2014 "Technical Documentation" [7].

Processed Sensor Data

FilterPredefiend Obstacle Map Balrog States

Figure 16: A sketch of the positioning and mapping system (arrows denote information flow)

The positioning and mapping system consists of:

• Pre-filter processing

• Filter

5.4.1 Pre-filter Processing

Not all sensors provide data that is suitable for direct use in the filter. Values from these sensorsare pre-processed to make them usable. Currently, almost all preprocessing is done in the SDSPsubsystem. Only the barometer data is preprocessed in positioning and mapping, and as thebarometer is not used by the filter, this has only been implemented for future use. The pressureas measured with the barometer is used to calculate Balrog’s vertical velocity. This is done bytaking the slope of a first order linear regression on the barometer readings taken since last filteriteration. This gives the change in pressure during the time since last filter iteration, which isconverted to a velocity by multiplication with a constant that depends on air pressure. For futuredevelopment, the barometer preprocessing might be moved to the SDSP subsystem as with theother sensors, or, if the barometer is deemed unnecessary, entirely removed.

5.4.2 Filter

The filter is a marginalized particle filter [15]. The development of the filter was started in2014. For a full description of the filter algorithm and its implementation (including the loopparallelisation), see the technical documentation from the 2014 project [7]. Observe that thenumerical issues from last year have been solved.

To estimate Balrog’s states, the filter uses a motion model and a measurement model. The mo-tion model contains information about the dynamics of Balrogs states, and the measurement



LIPsPage 31


2015-12-07model contains information about how the relationship between the states and the measure-ments. The only sensor not handled by the measurement model is the accelerometer data, thatis regarded as input and used to calculate the predicted states. An obstacle map must be definedin Balrog for the range sensors to work correctly. Balrog compares the range sensor readingswith this map to estimate its states. The given map is regarded as absolute and will not bechanged by Balrog. If no map is predefined, the range sensors must not be used by the filter.The filter also calculates expected sensor readings with variances for the SDSP subsystem.

It is possible to run the filter offline by adding the subscript "-filteroffline" when starting theBalrog executable. This should be possible both on the platform and on a PC. During offlineruns, the filter gets its sensor data from the .txt files specified in the measurement model insteadof from the SDSP subsystem. A Matlab script that simulates Balrog and generates these .txtfiles has been implemented. Another Matlab script can compare the "true" states simulatedin Matlab with the estimated states from Balrogs log files. Further development of the offlinefunctionality is possible. The .txt files could be changed to look similar to Balrogs log files.This would make it easy to test the filter on real sensor data. Also, a Simulink model of Balrogcould be implemented. This would probably make the simulation easier to understand and todevelop further.

5.4.2.1 Motion Model

A motion model is used by Balrog. The model is based on a coordinated turn 2D motion modelwith polar velocity. The model also contains the roll and pitch of Balrog, as these states areuseful in modelling the accelerometer (but this is not implemented). The states at time k, xk aregiven below:

xk =

xkykψk

φk

θkvkωk

(6)

where x is Balrog’s x coordinate, y is Balrog’s y coordinate, φ its roll, θ its pitch, ψ is its yaw,v its velocity and ω its yaw angle velocity. The first three states, x, y and φ, are defined asnonlinear in the filter, and the last four are linear. Currently, the states φ and θ (roll and pitch)are parts of the model but they don’t effect the dynamics of the other states or the measurements.

The time-discrete dynamics of the model is given by:

xk+1 =

xk + Tvk cos(ψk)yk + Tvk sin(ψk)

ψk + Tωk

φk

θkvkωk

+

T 2

2cos(ψ) 0 0

T 2

2sin(ψ) 0 00 0 00 T 00 0 TT 0 00 0 0

uk+

T 2

2cos(ψ) 0 0 0

T 2

2sin(ψ) 0 0 0

0 T 2

20 0

0 0 T 00 0 0 TT 0 0 00 T 0 0

wk (7)



LIPsPage 32


2015-12-07The vector uk contains the system inputs. u1k is Balrog’s acceleration (measured by the IMU), u2kis the roll angle speed (measured by the gyroscope) and u3k is the pitch angle speed (measured bythe gyroscope). The vector wk contains the process noise. w1

k is the IMU acceleration noise, w2k

is the yaw angle acceleration (modelled as noise), w3k is the noise in the roll speed measurement

and w4k is the noise in the pitch speed measurement.

Currently, the accelerometer value u1k is taken without regard for gravity. This works whenrunning the filter offline, but will give erroneous values i practice, as Balrog will not be per-fectly horizontal at all times. This is fixed if Balrogs pitch (θ) is taken into account. This canbe done by subtracting Tθg from the velocity update, T 2

2cosψθg from the x position update

and T 2

2sinψθg from the y position update in the motion model dynamics. This has not been

implemented.

5.4.2.2 Measurement Model

As the SDSP subsystem handles the more complicated sensor modelling, only simple sensormodels is be needed. The sensor inputs are:

yk =

XGPS

YGPS

ωgyroscopevR+vL

2vR−vL

dv

rultrasonic,1rultrasonic,2rultrasonic,3rultrasonic,4rultrasonic,5rultrasonic,6

(8)

XGPS and YGPS are the GPS position measurements, ωgyroscope is the IMU angle velocity mea-surements, vR and vL are the odometer measurements, dv is the virtual length of Balrog definedby the group and rultrasonic,k is the ultrasonic range measurement for sensor k. Only the sensorreadings that SDSP deems accurate is used in the filter, which means that yk might not containall of the readings stated above. They are connected to the states according to:

h(xk) =

xkykωk

vkωk

lultrasonic(1, xk, yk, ψk)lultrasonic(2, xk, yk, ψk)lultrasonic(3, xk, yk, ψk)lultrasonic(4, xk, yk, ψk)lultrasonic(5, xk, yk, ψk)lultrasonic(6, xk, yk, ψk)

(9)



LIPsPage 33


2015-12-07lultrasonic(k, xk, yk, ψk) are functions that given Balrog’s position, yaw and obstacle map (prede-fined or dynamic) calculates the distance from Balrog’s position (as given by the state vector) tothe nearest wall in the direction of the measurement. Each k is associated with an angle at whichthe sensor k is mounted. The range function lultrasonic() can be developed further to account forthe different position of Balrogs sensors (right now all sensors are expected to be positioned atthe IMU) and to handle when no obstacle is detected (eg. the sensor gives a maximal reading).

The lultrasonic,k(x) algorithm runs as follows:

• Shift the obstacle map to make Balrog’s position (x, y) = (0, 0)

• Rotate the obstacle map with the measurement angle, so that the measurement directionbecomes (1, 0).

• Discard all lines whose endpoints has negative x-coordinates (lines behind Balrog).

• Discard all lines whose endpoints has non-zero y-coordinates with the same sign (linesabove/below the measurement line).

• Calculate the intersection between the measurement line (0, 0) + (1, 0)t, t > 0 and allremaining line segments. The answer is on the form (R, 0), where R is the distance to theintersection. This distance is saved. For some lines no intersection is found, the algorithmskips to the next line segment in that case.

• Return the smallest distance R found. If no R (e.g. no intersections) was found, return aspecific distance larger then the range of the ultrasonic sensor to indicate this.

The algorithm has been tested and works in practise, but is not optimized for speed. messuremntwas taken might have been different due to movement of balrog. Hopefully, this is so smalltheres no practical effect. Esp. for the ultrasonic sensors...

The possibility for the measurement model to also contain inputs from the barometer has beenimplemented, but this is not used as it uses the vertical speed of Balrog, which is obtained bymultiplying the velocity with the pitch. If this is used, either the pitch or the velocity must be anon-linear state for the MPF theory to apply.

The measurement model updates the read flag on Processed<Sensor>Sample (insertsensor name instead of <Sensor>) when it reads a value. Currently this is done by adding anew sensor data object to the shared buffer with the read flag set. This works, but is unintuitive,makes the log files less clear and will probably cause problems down the road. As the theread flag makes sure that the filter doesn’t read the same value twice, this needs to be solved.A solution could be to implement a flag in the data collection object instead of in the sensorsample.

each point has been visited is stored. New estimates of Balrog’s positions and their uncertaintieswill be used to update the map. Old probabilities must be updated when new information arises.The map will have a resolution of 10 cm. The probability map will be implemented accordingto last years technical documentation [7]. quadratic room with an obstacle in one corner. Saythat Balrog sees an empty corner. Balrog might then be in either of the 3 empty corners and theyshould be given the same probabilities (33and finds that the next corner is also empty. Then itmust be able to figure out that it didn’t start in the corner next to the object. If Balrog continuesto explore clockwise and finds that the next corner is also empty, it should know its possition.How do we implement that? Maybe the filter will handle it...



LIPsPage 34


2015-12-075.4.3 Implementation and flow in subsystem

Information flows in to the positioning and mapping subsystem from the SDSP subsystem andfrom the predefined map. The data is sent to the filter. It estimates Balrog’s states and pre-dicts future sensor readings for the SDSP subsystem. An overview of the system was given inFigure 16 earlier.

5.4.4 Dependencies to other subsystems

The Positioning and Mapping Subsystem is dependent on data from the SDSP subsystem. Thesubsystem is dependent on that the predefined map is correct, and map errors should be as smallas possible. If no predefined map is available, the range sensors must not be used by the filter(for example, by setting them as unreliable in the SDSP subsystem).

The positioning and mapping runnable creates a marginalized particle filter object and initializesit with parameters such as Balrog’s starting position and the number of particles used. Theupdate_states() method of the filter object is then run regularly to estimate new states, andthis method is the basis of the positioning and mapping system. The marginalized particlefilter creates a measurement model object and a motion model object. The measurement modelcontains methods for handling the measurements. The motion model object contains methodsfor the handling of Balrogs dynamics.

The positioning and mapping subsystem was called the "SLAM subsytem" in previous yearsdocumentation. It then also included routines for object detection and mine detection. Theobject detection was based on a laser scanner (LIDAR) that was not used this year and on thepremise that no predefined map was available and was not relevant for this years work. Themine detection subsystem was not in focus this year and has not been touched. Its performanceis unknown.



LIPsPage 35


2015-12-075.5 Implementation Overview

This section presents the implementation of Balrog. In figure 17 all hardware, subsystems, dataclasses and external systems are presented. Figure 17 shows which parts that communicate andwhich shared data the subsystems need.

Figure 17: Overview of the system and how it communicates

5.5.1 Shared data structure

This year’s project have kept the design of shared data from last year [6]. The data is sharedbetween different subsystems by passing in a reference to the data when the subsystem is instan-tiated. All data manipulation (such as add and get) are implemented to be safe for concurrency,race condition etc. This is done in the class DataCollection created last year.

All shared data structures inherits from ASerializable, meaning it is possible to serializeand deserialize the object to and from a stream, making it possible to send them via the datalink. We have experienced some minor bugs in this implementation from last year and havefixed the problems that we are aware of. The bugs came from typecasting uint to int or viceversa and also from not sending " " to the stream, corrupting the fragmentation of the message.There is a test though that is commented out in StatesTest.cpp called SerializeStates (notice the



LIPsPage 36


2015-12-07s in the end as it is a similar to the test SerializeState). We haven’t put any effort in fixing thisas we haven’t used this feature in our implementation and the bug was noticed in a late stage ofthe project where other tasks were prioritized.

5.5.2 Subsystems

Implementation of subsystems follows the design from last year [6]. Subsystems inherit fromthe class ARunnable and are all completely isolated subsystems. As long as they are instanti-ated with the correct parameters (such as references to shared data structures and other subsys-tems), they are able to run independently of the other subsystems. This allows a high degree ofdecoupling between subsystems and they can easily be tested and developed in parallel.

Each subsystem runs in its own thread. The thread instantiation is taken care of automaticallyin the ARunnable class and is nothing we need to consider when using the objects. Usuallysome of the shared data structures are passed as arguments to the constructor. A subsystemmight need to be able to read the shared data or even to manipulate it.

Subsystem Communication on balrog have been changed to run periodically from last yearwhen it was ran as often as possible. Last year’s implementation had a function that waitedfor data from base station in order to send data to the base station. We don’t think this wasintentional because this resulted in the risk that no data was sent from balrog if no commandswere sent from the base station. When this function were changed, the base station was insteadflooded with data. This was the reason that the thread was changed to run periodically but thiscan results in latencies of commands sent from the base station (nothing we experienced but therisk is there). We recommend next year to instead run this thread as often as possible but checkwhether the data are new to avoid sending multiple copies of the same data.



LIPsPage 37


2015-12-07

A Appendix: Sensor Characteristics

Here the results of the noise modelling are presented. Table 10, 11 and 12 show the estimatedvariances of the measurement noise.

All estimated variances for the IMU are based on four data sets where one experiment was madeoutside. Balrog was standing still on flat ground. The data set used for the odometer was madewhen Balrog was rotating on the same spot, the left and right velocity was constant. An othermeasurement for the odometer was made when Balrog was standing still, all data was zero asexpected. The GPS measurement noise was modelled from data sets when Balrog was standingstill outside. Two data sets were taken. The ultrasonic sensor measurements were made instationary for each sensor at four different distances against a wall. As mentioned i Section5.3.1 one and same variance for the sensors was chosen.

Sensor variance x variance y variance zAccelerometer 1.0435e-05 1.2221e-05 9.3496e-06Gyroscope 2.9861e-07 2.2154e-07 1.9815e-07Magnetometer 7.6524e-05 1.1146e-04 3.6418e-05GPS 0.2400 0.4166 0.0279

Table 10: Noise variances

Sensor VarianceUltrasonic front left 0.073Ultrasonic front right 0.073Ultrasonic left 0.073Ultrasonic right 0.073


Sensor ∆distance,left ∆distance,right velocity left velocity rightOdometer 3.6916e− 06 1.0776e− 06 2.1725e− 04 2.6475e− 04


For the GPS an inverse filter was modelled and the coefficients of the first order AR-model arepresented in Table 13.



LIPsPage 38


2015-12-07

coefficient x y za1 -0.9838 -0.9866 -0.9840

Table 13: Coefficients AR-model GPS



LIPsPage 39


2015-12-07

B Appendix: Message Id’s

Name ID ID no.Legacy Id’sNETWORK_MESSAGE_HEARTBEAT 0NETWORK_MESSAGE_STOP 1NETWORK_MESSAGE_RESET 2NETWORK_MESSAGE_SET_MODE_MANUAL 3NETWORK_MESSAGE_SET_MODE_AUTOMATIC 4NETWORK_MESSAGE_SET_MODE_SEARCH 5NETWORK_MESSAGE_WHEEL_SPEED 6NETWORK_MESSAGE_WAYPOINTS 7NETWORK_MESSAGE_GPS_SET_ORIGIN 8NETWORK_MESSAGE_GPS_USE_CURRENT_POSITION_AS_ORIGIN 9NETWORK_MESSAGE_GPS_GOT_FIX 10NETWORK_MESSAGE_GPS_LOST_FIX 11NETWORK_MESSAGE_GPS_ON 12NETWORK_MESSAGE_GPS_OFF 13NETWORK_MESSAGE_GPS_STATUS 14NETWORK_MESSAGE_PLANNER_STATUS 15NETWORK_MESSAGE_MINE_POSITION 16NETWORK_MESSAGE_STATE_DATA 17NETWORK_MESSAGE_SENSOR_DATA 18NETWORK_MESSAGE_OPERATIONAL_AREA 19NETWORK_MESSAGE_PROBABILITY_MAP 20NETWORK_MESSAGE_OBSTACLE_MAP 21NETWORK_MESSAGE_XBOX_CONNECTED 22NETWORK_MESSAGE_XBOX_BUTTON_MAP 23NETWORK_MESSAGE_GUI_WHEEL_SPEED 24NETWORK_MESSAGE_CONTROL_PARAMETERS 25NETWORK_MESSAGE_FILTER_PARAMETERS 26



LIPsPage 40


2015-12-07

Sensor sample Id’sNETWORK_MESSAGE_GPS_SAMPLE 27NETWORK_MESSAGE_ACCELEROMETER_SAMPLE 28NETWORK_MESSAGE_BAROMETER_SAMPLE 29NETWORK_MESSAGE_GYROSCOPE_SAMPLE 30NETWORK_MESSAGE_MAGNETOMETER_SAMPLE 31NETWORK_MESSAGE_ODOMETER_SAMPLE 32NETWORK_MESSAGE_LASER_SAMPLE 33Processed sensor sample Id’sNETWORK_MESSAGE_GPS_SAMPLE_SDSP 34NETWORK_MESSAGE_ACCELEROMETER_SAMPLE_SDSP 35NETWORK_MESSAGE_BAROMETER_SAMPLE_SDSP 36NETWORK_MESSAGE_GYROSCOPE_SAMPLE_SDSP 37NETWORK_MESSAGE_MAGNETOMETER_SAMPLE_SDSP 38NETWORK_MESSAGE_ODOMETER_SAMPLE_SDSP 39NETWORK_MESSAGE_LASER_SAMPLE_SDSP 40NETWORK_MESSAGE_SENSOR_TOGGLE 41Ultrasonic Id’sNETWORK_MESSAGE_ULTRASONIC_SAMPLE_FRONT_LEFT 42NETWORK_MESSAGE_ULTRASONIC_SAMPLE_FRONT_RIGHT 43NETWORK_MESSAGE_ULTRASONIC_SAMPLE_LEFT 44NETWORK_MESSAGE_ULTRASONIC_SAMPLE_RIGHT 45NETWORK_MESSAGE_ULTRASONIC_SAMPLE_FRONT_LEFT_SDSP 46NETWORK_MESSAGE_ULTRASONIC_SAMPLE_FRONT_RIGHT_SDSP 47NETWORK_MESSAGE_ULTRASONIC_SAMPLE_LEFT_SDSP 48NETWORK_MESSAGE_ULTRASONIC_SAMPLE_RIGHT_SDSP 49Map Id’sNETWORK_MESSAGE_LINE_MAP 50



LIPsPage 41


2015-12-07

References

[1] LIPS – nivå 1. Version 1.0. Tomas Svensson och Christian Krysander. Compendium,LiTH, 2002.

[2] MTi User Manual, Xsens Technologies B.V., Netherlands, Document MT0605P, RevisionF, 27 February 2015.

[3] MT Low Level Communication Documentation, Xsens Technologies B.V., Netherlands,Document MT0101P, Revision T, 27 February 2015

[4] Designspecifikation, Johan Norberg, http://www.isy.liu.se/edu/projekt/tsrt71/2009/bandvagn/doc/Designspecifikation.pdf , Revision 1.0, 1October 2009

[5] Requirement Specification, Marcus Bäck, http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/Requirement_specification_v1.0.pdf , Revision 1.0, 19 September 2014

[6] Design Specification, Johan Källström, http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/TSRT10___Designspec.pdf , Revision 1.0, 27 September 2014

[7] Technical Documentation, Marcus Bäck, http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/Documents/TechnicalDocumentation.pdf , Revision 1.0, 11 December 2014

[8] User Manual, Oscar Hörberg, http://www.isy.liu.se/edu/projekt/tsrt10/2015/bandvagn/UM.pdf , Revision 1.0, 8 December 2015

[9] SRF10 Ultrasonic range finder Technical Specification, Robot Electronics, http://www.robot-electronics.co.uk/htm/srf10tech.htm

[10] Ultrasonic Rangers FAQ http://www.robot-electronics.co.uk/htm/sonar_faq.htm

[11] Arduino Wire Library https://www.arduino.cc/en/Reference/Wire

[12] Arduino Uno Overview https://www.arduino.cc/en/Main/ArduinoBoardUno

[13] BL233 Datasheet, I2Cchip http://www.i2cchip.com/pdfs/bl233_b.pdf

[14] System Identification Toolbox, MATLAB http://se.mathworks.com/help/ident/

[15] Statistical Sensor Fusion, Fredrik Gustafsson, Studentlitteratur AB, 2012 p3cm |



LIPsPage 42

http://www.isy.liu.se/edu/projekt/tsrt71/2009/bandvagn/doc/Designspecifikation.pdf

http://www.isy.liu.se/edu/projekt/tsrt71/2009/bandvagn/doc/Designspecifikation.pdf

http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/Requirement_specification_v1.0.pdf



http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/TSRT10___Designspec.pdf



http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/Documents/TechnicalDocumentation.pdf



http://www.isy.liu.se/edu/projekt/tsrt10/2015/bandvagn/UM.pdf

http://www.isy.liu.se/edu/projekt/tsrt10/2015/bandvagn/UM.pdf

http://www.robot-electronics.co.uk/htm/srf10tech.htm

http://www.robot-electronics.co.uk/htm/srf10tech.htm

http://www.robot-electronics.co.uk/htm/sonar_faq.htm

http://www.robot-electronics.co.uk/htm/sonar_faq.htm

https://www.arduino.cc/en/Reference/Wire

https://www.arduino.cc/en/Main/ArduinoBoardUno

https://www.arduino.cc/en/Main/ArduinoBoardUno

http://www.i2cchip.com/pdfs/bl233_b.pdf

http://se.mathworks.com/help/ident/

http://se.mathworks.com/help/ident/