Acoustics

Dropbox link for all related MDP work:

https://www.dropbox.com/sh/rz6363bwxtjmwb9/AADyNuaTvNP3c6HurQCM-X94a?dl=0

Introduction
Underwater acoustic localisation has been a topic of interest to many scientists, and continuous research over decades has resulted in improvement and more understanding about its characteristics and robustness as compared to BEACONs. Sound wave is the only solution we have so far to look into the issue of underwater acoustic localisation. Phase Difference and Time of Arrival (ToA) of the sound wave received from two or more hydrophones are being observed and interpreted for data and analysis. In the real world, underwater localisation is not feasible. Scientists and researchers are spending a lot of time to tackle and introduce possible solutions to it. Hence, we attempt to resolve this issue by introducing a real world application for the purpose of directing Autonomous Underwater Vehicle to the location of the source.

Localisation
Localisation underwater has always been a challenge due to limited information received at the receiving end. By making underwater localisation possible and accurate, tracking and searching will be possible to minimise the area of search to a smaller radius, simultaneously reducing costs for the search team. For example, a plane that crashes into an ocean will eventually sink; the black box on the plane will activate and will become an emitting source. Localisation will allow the rescue team to narrow down the area of the source for a faster and cost effective process.

Time of Arrival:

By introducing Time of Arrival of the signal, the distance between the hydrophone can be increased, allowing the current hydrophones to perform at their optimum performance. Time of Arrival of the signal at the two hydrophones will give us sufficient data to compute the direction of the source. From the time of arrival we will be able to obtain the difference in Time of Arrival between the two hydrophones:

*** FORMULA***

By obtaining the difference in Time of Arrival of the signal, there could be errors due to refresh rate of the Data Acquisition Board and showing the opposite direction of the signal. Hence, condition loop is introduced to make sure that the signal being computed is correctly done. Amplitude of the signal at the two hydrophones will be different. This gives us an insight to which side of the signal is coming from, as the amplitude of the signal will drop exponentially and the difference in amplitude can be seen clearly.

*** FORMULA***

Phase Difference:
Basic Trigonometry and physics are used to determine the direction and distance to the source. However, the size of the current hydrophones make it impossible to arrange it such that it is spaced apart by a maximum of half wavelength of the highest target frequency being listened to to achieve the phase difference of the signal receiving at the two hydrophones - in this case, 40kHz. The equation relating wavelength to frequency and propagation speed is:

*** FORMULA***

The speed of the sound in water is around 1500 m/s. Using the equation above, the half wavelength at 40kHz is:

*** FORMULA***

Therefore, the distance between the two hydrophones has to be less than or equal to 1.875 cm. The distance cannot be achieved with the current equipment we are using hence we have to look from another perspective and come out with another solution. ToA has to be introduced to make the computation of direction possible.

Direction:
The Phase Difference of the signal can be computed and therefore fit into the equation deduced from experimental data:

*** FORMULA***

By substituting Phase Difference obtained into direction equation, direction angle is from North of the robot to the direction.

Distance:
Amplitude of the signal received at the receiving end is able to give us information of the distance between source and receiver. Hence, experiment was conducted to collect data of the amplitude of the signal from at 2 to 8m at 2m interval. Datas collected are put onto the graph and best fit curve is drawn. The equation of the curve is:

*** FORMULA***

Kalman Filter
Kalman Filtering is an iterative mathematical tool for predicting the absolute value of a valuable given a lot of data points for variable coming in over time, each data set having errors, uncertainty or variance.

*** Picture***

Kalman Gain
Dimensionless parameters that determine and update with every periodic data input to “decide” whether the new estimate should have more contribution from the Measured Value or the Previous Estimate.

Kalman Gain is to be defined first and be calculated based on the errors in both estimate and measurement. The initial value of Error in Estimate and Error in Measurement are predetermined from the past experiments as they usually is based on error of the instruments used.

*** FORMULA***

Current Estimate
Current Estimate is an estimation based on computation of previous estimate, kalman gain, and measured value to give a possibility of a more accurate value. Current estimate gets more accurate and reliable over time.

*** FORMULA***

Error in Estimate
== Error in Estimate is the difference between the current estimate and the true value. However, it can be minimised mathematically so that the Current Estimate can be as close as the true value. The prediction of the next error in estimate is based on the current kalman gain and the current error in estimate.

*** FORMULA***

Error in Measurement
== Error in measurement is inevitable due to existing noise which fluctuates as a result of many factors. Nonetheless, the error in measurement is preferably kept as a constant for easy computation as the error in measurement will not differ much as the environment and instruments are always similar.

Decision Making
In its simplest sense, decision-making is the act of choosing between two or more courses of action. In the wider process of problem-solving, decision-making involves choosing between possible solutions to a problem. Decisions can be made through either an intuitive or reasoned process, or a combination of the two. However, in the robot decision making, it only can make a reasoning decision making based on data it received using the algorithm and probability to see the reliability of the system. In this case, with Kalman Filtering, the robot has to decide whether to believe the Current Estimate value or the Measured Value. This mathematical process of decision making will allow the robot to reach the true value at a faster and more accurate pace.

*** Picture***

Kalman Gain can give us the information on what is more reliable, or in this case, has less error. A Kalman Gain closer to one means the measured value is more reliable, and when it is closer to zero, it means that the estimated value is more reliable. Hence, the robot decision will be based on Kalman Gain computed at that time and it will move according to the decision.

*** FORMULA***

This allows the robot to decide on its own whether it should believe data they are receiving for the best movement towards the source. However, when the system and algorithm becomes more reliable, the probability of the decision formulae will change as the median of the kalman gain curve has shifted and gets closer to one.

Labview
Data Acquisition Board is running on Labview software which is another coding language using block diagram. It helps us to import, store and supply data to ROS data base for subscriber. The Labview Programme created are based on the time the signal is received at each Hydrophone. From that, the programme will calculate the phase difference of the two signals and then the direction to the source. ==

Robotic Operating System
The Singapore Polytechnic’s Autonomous Underwater Vehicle (AUV) is operated by Robotic Operating System or ROS. It is a system allowing the data to be published and subscribed for the individual usage of the equipment. For example, Depth Sensor is getting data of the how far away from the robot to the bottom of the pool. It is then published onto ROS Data Base and Thruster Driver which subscribed to that particular data is receiving data to maintain the robot depth and ensure it is at the correct depth set by the user. ==

1. Data Acquisition
DAQ Board is connected to the Odriod of the AUV via RS232 to USB. However, a driver has to be written so that the data that is being transferred from DAQ Board to Odriod can be published onto ROS Data Base. Hence, a simple C++ programme of publishing serial data is written and installed onto sp_auv file. This will allow the direction data to be published and whichever system requires the direction will subscribe to the particular file. Refer to DAQ Code.

*** Picture***

2. Kalman Filtering

Kalman Filter is run on ROS to minimise the performance of the DAQ Board, to ensure the highest sampling rate the DAQ Board can offer so that data can be collected quickly and has minimal chances of missing any data.

The flowchart process below will execute for 5 seconds as initialisation time to allow a more accurate true value. Then, it is compared to decision making to observe which has less error and trust that error for publishing onto ROS Data Base. Additionally, the published data is being subscribed by compass heading so that it changes its direction and the robot will head towards the source. Refer to Kalman Filter Code. ==

DAQ Code:
== //tty usb /dep

//usb 0 = depth

// 1 = compass

// 2 or 3 for daq

//


 * 1) include


 * 1) include


 * 1) include

// OS Specific sleep


 * 1) ifdef _WIN32


 * 1) include 


 * 1) else


 * 1) include 


 * 1) endif


 * 1) include "serial/serial.h"


 * 1) include "ros/ros.h"


 * 1) include "std_msgs/String.h"


 * 1) include "std_msgs/Float32.h"


 * 1) include

using std::string;

using std::exception;

using std::cout;

using std::cerr;

using std::endl;

void my_sleep(unsigned long milliseconds) {


 * 1) ifdef _WIN32

Sleep(milliseconds); // 100 ms


 * 1) else

usleep(milliseconds*1000); // 100 ms


 * 1) endif

}

int run(int argc, char **argv)

{

if(argc < 3) {

cerr << "Usage: serial_read ";

cerr << " [test string]" << endl;

return 0;

}

ros::init(argc, argv, "DAQSounder"); //1

ros::NodeHandle n;

ros::Publisher daq_pub = n.advertise("DAQSounder", 1000); //1

ros::Rate loop_rate(10);

ros::NodeHandle nh("~");

std::string getval;

std::string def_port="/dev/ttyUSB5";

std::string def_baud="9600";

nh.param("port",getval,def_port);

string port(getval);

nh.param("baud",getval,def_baud);

// Argument 1 is the serial port

// Argument 2 is the baudrate

unsigned long baud = 0;


 * 1) if defined(WIN32) && !defined( __MINGW32__ )

sscanf_s(getval.c_str, "%lu", &baud);


 * 1) else

sscanf(getval.c_str, "%lu", &baud);


 * 1) endif

// port, baudrate, timeout in milliseconds

serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));

cout << "Is the serial port open?";

if(my_serial.isOpen)

cout << " Yes." << endl;

else

cout << " No." << endl;

// Get the Test string

int count = 0;

string test_string;

if (argc == 4) {

test_string = argv[3];

} else {

test_string = "Testing.";

}

// Test the timeout, there should be 1 second between prints

//cout << "Timeout == 1000ms, asking for 1 more byte than written." << endl;

while (ros::ok) {

string result = my_serial.readline;

char tmptxt[20]="";

std_msgs::String msg;

std::stringstream ss;

std_msgs::Float32 daq; //std_msgs

std::size_t length = result.copy(tmptxt,6,0);

tmptxt[length]='\0';

daq.data = atof(tmptxt); //data

ss << tmptxt ;

msg.data = ss.str;

ROS_INFO("%s", msg.data.c_str);

daq_pub.publish(daq); //publish

ros::spinOnce;

loop_rate.sleep;

//count += 1;

}

return 0;

}

int main(int argc, char **argv) {

try {

return run(argc, argv);

} catch (exception &e) {

cerr << "Unhandled Exception: " << e.what << endl;

}

==

Kalman Filter Code:
==
 * 1) include "ros/ros.h"


 * 1) include "std_msgs/String.h"


 * 1) include "std_msgs/Float32.h"


 * 1) include "std_msgs/Int16.h"


 * 1) include


 * 1) include


 * 1) include


 * 1) include


 * 1) include 


 * 1) include


 * 1) include


 * 1) define Error_mea 10 //Constant

double Kalman_Gain

double EST_t

double EST_t_1

double MEA

double Error_est = 10

double Error_est_new

int direction

int direction_pub

using std::string;

using std::exception;

using std::cout;

using std::cerr;

using std::endl;

using namespace std;

void CompassCallback(const geometry_msgs::Pose2D::ConstPtr& msg)

{

heading = msg->theta;

}

void DAQCallback(const std_msgs::Float32::ConstPtr& msg)

{

direction = msg->theta;

}

int main (int argc, char **argv)

{

ros::init(argc, argv, "KalmanFilter");

ros::NodeHandle n;

std_msgs::Int16 Direction;

int count = 0;

ros::Subscriber sub_compass = n.subscribe("SpartonCompass", 1000, CompassCallback);

ros::Subscriber sub_daq_direction = n.subscribe("DAQ", 1000, DAQCallback); //a c++ program to publish data collected from DAQ

ros::Publisher direction_pub = n.advertise("KalmanFilter", 500);

ros::Rate loop_rate(10);

while (ros::ok)

{

ros::spinOnce; //will call all the callbacks waiting to be called at that point in time.

/*

initialise first for x seconds

then will just keep running to keep it on track

1.initialise

2.a direction found

3.move accordingly

4.Direction 0 should be obtained all the way

5.Use optics to find yellow pole (in the statemachine)

* /

//initialise

void foo(void)

{

time_t end = time(NULL) + 5; //initialisation time (5s)

while (time(NULL) <= end)

{

Kalman_Gain = Error_est / (Error_est + Error_mea);

EST_t = EST_t_1 + Kalman_Gain * (MEA - EST_t_1);

Error_est_new= (1 - Kalman_Gain)*Error_est;

Error_est = Error_est_new;

}

}

// Decision making with 50/50 chance on the decision of the clean data of direction to be published onto the ROS data base

if (0.5 < Kalman_Gain < 1)

{

direction_pub = MEA;

}

if (0 < Kalman_Gain < 0.5)

{

direction_pub = EST_t;

}

// Publishing 1st data

string result = my_serial.readline;

char tmptxt[20]="";

std_msgs::String msg;

std::stringstream ss;

std_msgs::Float32 direction;

std::size_t length = result.copy(tmptxt,6,0);

tmptxt[length]='\0';

direction.data = atof(tmptxt);

ss << tmptxt ;

msg.data = ss.str;

ROS_INFO("%s", msg.data.c_str);

direction_pub.publish(direction);

ros::spinOnce;

loop_rate.sleep;

while (true)

{

Kalman_Gain = Error_est / (Error_est + Error_mea);

EST_t = EST_t_1 + Kalman_Gain * (MEA - EST_t_1);

Error_est_new= (1 - Kalman_Gain)*Error_est;

Error_est = Error_est_new;

if (0.5 < Kalman_Gain < 1)

{

direction_pub = MEA;

}

if (0 < Kalman_Gain < 0.5)

{

direction_pub = EST_t;

}

//publish data only if the direction_pub is out of range of tolerance

if (-5 >= direction_pub && 5 <= direction_pub)

{

string result = my_serial.readline;

char tmptxt[20]="";

std_msgs::String msg;

std::stringstream ss;

std_msgs::Float32 direction;

std::size_t length = result.copy(tmptxt,6,0);

tmptxt[length]='\0';

direction.data = atof(tmptxt);

ss << tmptxt ;

msg.data = ss.str;

ROS_INFO("%s", msg.data.c_str);

direction_pub.publish(direction);

//did not have ros::spinOnce here, your callbacks would never get called

ros::spinOnce;

loop_rate.sleep;

}

}

}

==