/* Function Definitions */
void mfcc_bare(const real_T samples_in_window[128], const real_T hamming_coeff
[128], const real_T mel_filterbank[2016], real_T fftA, real_T
fftB, const creal_T dct_coeff[32], creal_T mel[13])
{
real_T b_samples_in_window[128];
int32_T i3;
creal_T samples_in_freq[128];
int32_T i4;
int32_T i5;
int32_T i6;
int32_T ar;
int32_T ib;
creal_T samples_in_freq_data[65];
int32_T tmp_data[65];
int32_T ia;
int32_T loop_ub;
int32_T samples_in_freq_size[2];
int32_T tmp_size[2];
real_T b_tmp_data[65];
real_T b_data[65];
real_T melfilter_resps[32];
real_T b_melfilter_resps[32];
creal_T dcv0[32];
int32_T c_tmp_data[65];
int32_T b_samples_in_freq_size[2];
creal_T dc0;
/* initialization */
/* 8kHz sampling frequency. */
/* 128 (size of windows). */
/* 64 step size for subsequent windows. */
/* 26 (# of mel frequency pass band filters). */
/* overrride this to power of 2. */
/* # 12 (# of cepstral coefficients (output features)) */
/* compute features for one input window */
for (i3 = 0; i3 < 128; i3++) {
b_samples_in_window[i3] = samples_in_window[i3] * hamming_coeff[i3];
}
fft(b_samples_in_window, samples_in_freq);
if (fftA > fftB) {
i3 = 1;
i4 = 0;
} else {
i3 = (int32_T)fftA;
i4 = (int32_T)fftB;
}
if (fftA > fftB) {
i5 = 1;
i6 = 0;
} else {
i5 = (int32_T)fftA;
i6 = (int32_T)fftB;
}
if (fftA > fftB) {
ar = 1;
ib = 0;
} else {
ar = (int32_T)fftA;
ib = (int32_T)fftB;
}
ia = (ib - ar) + 1;
loop_ub = ib - ar;
for (ib = 0; ib <= loop_ub; ib++) {
tmp_data[ib] = ar + ib;
}
samples_in_freq_size[0] = 1;
samples_in_freq_size[1] = ia;
loop_ub = ia - 1;
for (ar = 0; ar <= loop_ub; ar++) {
samples_in_freq_data[ar] = samples_in_freq[tmp_data[ar] - 1];
}
d_abs(samples_in_freq_data, samples_in_freq_size, b_tmp_data, tmp_size);
loop_ub = tmp_size[1] - 1;
for (ar = 0; ar <= loop_ub; ar++) {
b_data[ar] = b_tmp_data[ar];
}
if (tmp_size[1] == 1) {
for (ar = 0; ar < 32; ar++) {
melfilter_resps[ar] = 0.0;
for (ib = 0; ib < 63; ib++) {
melfilter_resps[ar] += mel_filterbank[ar + (ib << 5)] * b_data[ib];
}
}
} else {
memset(&melfilter_resps[0], 0, sizeof(real_T) << 5);
if (63 == tmp_size[1]) {
for (ar = 0; ar < 32; ar++) {
melfilter_resps[ar] = 0.0;
for (ib = 0; ib < 63; ib++) {
melfilter_resps[ar] += mel_filterbank[ar + (ib << 5)] * b_data[ib];
}
}
} else {
memset(&melfilter_resps[0], 0, sizeof(real_T) << 5);
ar = -1;
for (ib = 0; ib < 63; ib++) {
if (b_data[ib] != 0.0) {
ia = ar;
for (loop_ub = 0; loop_ub < 32; loop_ub++) {
ia++;
melfilter_resps[loop_ub] += b_data[ib] * mel_filterbank[ia];
}
}
ar += 32;
}
}
}
b_log(melfilter_resps);
/* Compute DCT */
for (ar = 0; ar < 16; ar++) {
b_melfilter_resps[ar] = melfilter_resps[ar << 1];
}
for (ar = 0; ar < 16; ar++) {
b_melfilter_resps[ar + 16] = melfilter_resps[31 + -2 * ar];
}
b_fft(b_melfilter_resps, dcv0);
for (ar = 0; ar < 32; ar++) {
melfilter_resps[ar] = dcv0[ar].re * dct_coeff[ar].re - dcv0[ar].im *
dct_coeff[ar].im;
}
ia = (i4 - i3) + 1;
loop_ub = i4 - i3;
for (i4 = 0; i4 <= loop_ub; i4++) {
tmp_data[i4] = i3 + i4;
}
loop_ub = i6 - i5;
for (i3 = 0; i3 <= loop_ub; i3++) {
c_tmp_data[i3] = i5 + i3;
}
b_samples_in_freq_size[0] = 1;
b_samples_in_freq_size[1] = ia;
loop_ub = ia - 1;
for (i3 = 0; i3 <= loop_ub; i3++) {
samples_in_freq_data[i3].re = samples_in_freq[tmp_data[i3] - 1].re *
samples_in_freq[c_tmp_data[i3] - 1].re - samples_in_freq[tmp_data[i3] - 1]
.im * -samples_in_freq[c_tmp_data[i3] - 1].im;
samples_in_freq_data[i3].im = samples_in_freq[tmp_data[i3] - 1].re *
-samples_in_freq[c_tmp_data[i3] - 1].im + samples_in_freq[tmp_data[i3] - 1]
.im * samples_in_freq[c_tmp_data[i3] - 1].re;
}
dc0 = sum(samples_in_freq_data, b_samples_in_freq_size);
c_log(&dc0);
for (i3 = 0; i3 < 32; i3++) {
b_melfilter_resps[i3] = melfilter_resps[i3] / 8.0;
}
mel[0] = dc0;
for (i3 = 0; i3 < 12; i3++) {
mel[i3 + 1].re = b_melfilter_resps[i3 + 1];
mel[i3 + 1].im = 0.0;
}
}
void tryToUnderstand1(RobotPosition * pose, std::list<Landmark *>* landmarks,std::vector<Landmark *> * last_observations, std::vector<Landmark *> * propagate_observations){
double rpose[3];
pose->getEstimateData(rpose);
propagate_observations->clear();
std::cout << "Robot Position:\t" << rpose[0] << "\t" << rpose[1] << "\t" << rpose[2] << "\n";
double expected[last_observations->size()]; // this will contain the projections of the previously seen landmarks in the current pose
// I.E. this tells where we expect to see the landmarks propagated from the last step
bool use_general_angle_tolerance[last_observations->size()]; // this will tell if the angle tolerance to be used is the general one or the bearing-only based one
int associations[pose->observations.size()]; // this will contain the candidate associations, that will become effective ONLY IF there will be no ambiguity
for(unsigned int i=0; i<pose->observations.size(); i++){
associations[i] = -1;
}
std::cout << "We expect to see stuff at:\n";
for(unsigned int i = 0; i<last_observations->size(); i++){ // populate the expected array
Landmark * lm = (*last_observations)[i];
if(lm->getObservations()->size() > 1){ // first case: landmark already has an estimated position
expected[i] = computeAngle(lm, pose);
use_general_angle_tolerance[i] = true;
}
else{ // second case: landmark has been seen only once up to now, hence there is no position estimation
double prev_seen = (*(lm->getObservations()))[0]->bearing;
double prev_theta = (*(lm->getObservations()))[0]->pose->theta();
double rotated = computeAnglesDifference(pose->theta(), prev_theta);
expected[i] = prev_seen-rotated;
use_general_angle_tolerance[i] = false;
}
std::cout << expected[i] << "\n";
}
std::cout << "observations:\n";
for(unsigned int obs_counter=0; obs_counter < pose->observations.size(); obs_counter++){ // for every observation in the current position
std::cout << (pose->observations)[obs_counter].bearing << ": ";
// Try to assign it to a previously generated landmark
if(last_observations->size() > 0){
unsigned int closer_index;
unsigned int second_closer_index;
// compute closer_index and second_closer_index.
double distances[last_observations->size()];
for(unsigned int i=0; i < last_observations->size(); i++){
distances[i] = d_abs(computeAnglesDifference(expected[i], pose->observations[obs_counter].bearing));
} // now 'distances' contains the distance between the currently considered observation and the expected values
closer_index = 0;
if(last_observations->size() == 1){
second_closer_index = 0;
}
else{
second_closer_index = 1;
if(distances[1] < distances[0]){
closer_index = 1;
second_closer_index = 0;
}
}
for (unsigned int i = 1; i < last_observations->size(); i++){
if (distances[i] < distances[closer_index]){
second_closer_index = closer_index;
closer_index = i;
}
else{
if(distances[i] < distances[second_closer_index]){
second_closer_index = i;
}
}
}
// if closer is "close enough" and second_closer is "far enough" (I.E. there is no ambiguity)
// associate the current observation to 'closer'.
double tolerance = GENERAL_ANGLE_TOLERANCE;
if(!use_general_angle_tolerance[closer_index]){
tolerance = BEAR_ONLY_ANGLE_TOLERANCE;
}
std::cout << "tolerance is " << tolerance << "\n";
if(distances[closer_index] < tolerance){
if((second_closer_index == closer_index) || (distances[second_closer_index] > SECOND_ANGLE_MIN_DISTANCE)){
associations[obs_counter] = closer_index;
std::cout << "close to " << expected[closer_index] << "\n";
}
else{
std::cout << "close to " << expected[closer_index] << " but also to " << expected[second_closer_index] << "\n";
}
}
else{
std:: cout << "is far from everything\n";
}
// NOTE: Do not add the observation to the set inside the landmark corresponding to
// 'closer' yet, because, if two or more NEW observations are associated to the same
// landmark, none of them will be added.
}
}
// add the computed observations to the landmarks, but only if there is no ambiguity
bool associated[last_observations->size()]; // associated[i] will be true if there is at least 1 new observation associated to the i-th landmark in last_observations
bool ambiguous[last_observations->size()]; // ambiguous[i] will be true if there are at least 2 new observations associated to the i-th landmark in last_observations
// initialize the arrays just created
for(unsigned int i=0; i<last_observations->size(); i++){
associated[i] = false;
ambiguous[i] = false;
}
// tell which ones are ambiguous
for(unsigned int i=0; i < pose->observations.size(); i++){
unsigned int value = associations[i];
if(value!=-1){
if (!associated[value]){
associated[value] = true;
}
else{ // associated is true, hence there is already a new observation (or more) polling for the landmark
ambiguous[value] = true;
}
}
}
// add non-ambiguous observations to the corresponding landmarks
// and create new landmarks for the new observed unassociated observations
std::cout << "adding observations to landmarks\n";
for(unsigned int i=0; i < pose->observations.size(); i++){
std::cout << "obs " << i << " ";
if(associations[i] != -1){
if(!ambiguous[associations[i]]){
std::cout << "tailed to " << associations[i] << "\n";
Landmark * lm = (*last_observations)[associations[i]];
lm->addObservation(&(pose->observations[i]));
lm->estimatePosition();
lm->checkConfirmed(_confirm_obs);
double newpos[2];
(*last_observations)[associations[i]]->getPosition(newpos);
std::cout << "\tnew position estimated:\t" << newpos[0] << "\t" << newpos[1] << "\n";
propagate_observations->push_back((*last_observations)[associations[i]]);
}
else{
std::cout << "should be tailed to " << associations[i] << ", but there is ambiguity\n";
}
}
else{ // unassociated
std::cout << "is a new landmark\n";
Landmark * lm = new Landmark();
lm->addObservation (&(pose->observations[i]));
// lm->estimatePosition();
landmarks->push_back(lm);
propagate_observations->push_back(lm);
}
}
std::cout << "now checking landmarks from previous step\n";
for(unsigned int i = 0; i<last_observations->size(); i++){
std::cout << "prev_obs " << i << " ";
Landmark * lm = (*last_observations)[i];
if(!associated[i]){
// for the landmarks expected to be seen that are not in the current set of observations, we have two cases:
if(lm->isConfirmed()){
// case one: the landmark has been confirmed observations, then it is reliable
std::cout << "was old enough, CONFIRMED\n";
}
else{
// case two: the landmark is pretty unreliable, then it is trashed
std::cout << "was too young, REMOVED\n";
landmarks->remove(lm);
delete lm;
}
}
else{
if(ambiguous[i]){
std::cout << "was ambiguous,";
if(lm->isConfirmed()){
std::cout << " but it was considered reliable, PROPAGATED" << std::endl;
propagate_observations->push_back(lm);
}
else{
std::cout << " and it was pretty young, REMOVED" << std::endl;
landmarks->remove(lm);
delete lm;
}
}
else{
std::cout << "has been REINFORCED\n";
}
}
}
std::cout << "\n\n";
}
/* Function Definitions */
static void b_equalizeOFDM(testMACRouterStackData *SD, const emlrtStack *sp,
const creal_T recv_data[1280], const real_T tx_longPreamble[53], const real_T
tx_pilots[48], const real_T c_tx_pilotLocationsWithoutGuard[4], const real_T
tx_dataSubcarrierIndexies_data[48], const int32_T
tx_dataSubcarrierIndexies_size[2], c_struct_T *estimate, OFDMDemodulator_1
*hPreambleDemod, OFDMDemodulator_1 *hDataDemod, creal_T R[576],
emxArray_creal_T *Rraw)
{
OFDMDemodulator_1 *obj;
const mxArray *y;
static const int32_T iv198[2] = { 1, 45 };
const mxArray *m49;
char_T cv241[45];
int32_T i;
static const char_T cv242[45] = { 'M', 'A', 'T', 'L', 'A', 'B', ':', 's', 'y',
's', 't', 'e', 'm', ':', 'm', 'e', 't', 'h', 'o', 'd', 'C', 'a', 'l', 'l',
'e', 'd', 'W', 'h', 'e', 'n', 'R', 'e', 'l', 'e', 'a', 's', 'e', 'd', 'C',
'o', 'd', 'e', 'g', 'e', 'n' };
const mxArray *b_y;
static const int32_T iv199[2] = { 1, 4 };
char_T cv243[4];
static const char_T cv244[4] = { 's', 't', 'e', 'p' };
const mxArray *c_y;
static const int32_T iv200[2] = { 1, 51 };
char_T cv245[51];
static const char_T cv246[51] = { 'M', 'A', 'T', 'L', 'A', 'B', ':', 's', 'y',
's', 't', 'e', 'm', ':', 'm', 'e', 't', 'h', 'o', 'd', 'C', 'a', 'l', 'l',
'e', 'd', 'W', 'h', 'e', 'n', 'L', 'o', 'c', 'k', 'e', 'd', 'R', 'e', 'l',
'e', 'a', 's', 'e', 'd', 'C', 'o', 'd', 'e', 'g', 'e', 'n' };
const mxArray *d_y;
static const int32_T iv201[2] = { 1, 5 };
char_T cv247[5];
static const char_T cv248[5] = { 's', 'e', 't', 'u', 'p' };
int8_T fullGrid[64];
int32_T k;
static const int8_T iv202[11] = { 0, 1, 2, 3, 4, 5, 59, 60, 61, 62, 63 };
int8_T ii_data[64];
int32_T ii;
boolean_T exitg2;
boolean_T guard2 = FALSE;
int8_T b_ii_data[64];
creal_T recv[64];
emxArray_creal_T *RLongFirst;
const mxArray *e_y;
static const int32_T iv203[2] = { 1, 45 };
const mxArray *f_y;
static const int32_T iv204[2] = { 1, 4 };
const mxArray *g_y;
static const int32_T iv205[2] = { 1, 51 };
const mxArray *h_y;
static const int32_T iv206[2] = { 1, 5 };
boolean_T exitg1;
boolean_T guard1 = FALSE;
creal_T b_recv[64];
emxArray_creal_T *RLongSecond;
emxArray_creal_T *b_R;
creal_T c_R[106];
real_T b_tx_longPreamble[106];
creal_T RNormal[106];
real_T dv13[106];
real_T dv14[106];
real_T REnergy[53];
creal_T RConj[53];
creal_T b_RConj[53];
const mxArray *i_y;
static const int32_T iv207[2] = { 1, 45 };
const mxArray *j_y;
static const int32_T iv208[2] = { 1, 4 };
const mxArray *k_y;
static const int32_T iv209[2] = { 1, 51 };
const mxArray *l_y;
static const int32_T iv210[2] = { 1, 5 };
int8_T b_fullGrid[768];
static const int16_T iv211[48] = { 11, 25, 39, 53, 75, 89, 103, 117, 139, 153,
167, 181, 203, 217, 231, 245, 267, 281, 295, 309, 331, 345, 359, 373, 395,
409, 423, 437, 459, 473, 487, 501, 523, 537, 551, 565, 587, 601, 615, 629,
651, 665, 679, 693, 715, 729, 743, 757 };
boolean_T c_fullGrid[768];
int32_T ii_size[1];
int32_T c_ii_data[768];
real_T d_ii_data[768];
int32_T b_ii_size[1];
creal_T RXPilots[48];
creal_T preambleGainsFull[636];
int32_T ia;
int32_T iv212[3];
static const int8_T iv213[3] = { 48, 12, 1 };
int32_T b_Rraw[3];
creal_T PilotNormal[48];
real_T pilotEqGains_re;
real_T pilotEqGains_im;
real_T a[48];
real_T PilotEnergy[48];
creal_T b_PilotNormal[48];
creal_T pilotEqGains[576];
creal_T preambleGainsFull_data[576];
creal_T b_preambleGainsFull_data[576];
creal_T c_preambleGainsFull_data[576];
real_T preambleGainsFull_data_re;
real_T preambleGainsFull_data_im;
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
emlrtStack d_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
d_st.prev = &c_st;
d_st.tls = c_st.tls;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
/* equalizeOFDM: Equalize the entire OFDM frame through the use of both */
/* the long preamble from the 802.11a standard and four pilot tones in */
/* the data frames themselves. The gains from the pilots are */
/* interpolated across frequency to fill the data frame and apply gains */
/* to all data subcarriers. */
/* %% Use Long Preamble frame to estimate channel in frequency domain */
/* Separate out received preambles */
emlrtVectorVectorIndexCheckR2012b(1280, 1, 1, 160, &y_emlrtECI, sp);
/* Demod */
st.site = &xr_emlrtRSI;
obj = hPreambleDemod;
if (!obj->isReleased) {
} else {
y = NULL;
m49 = mxCreateCharArray(2, iv198);
for (i = 0; i < 45; i++) {
cv241[i] = cv242[i];
}
emlrtInitCharArrayR2013a(&st, 45, m49, cv241);
emlrtAssign(&y, m49);
b_y = NULL;
m49 = mxCreateCharArray(2, iv199);
for (i = 0; i < 4; i++) {
cv243[i] = cv244[i];
}
emlrtInitCharArrayR2013a(&st, 4, m49, cv243);
emlrtAssign(&b_y, m49);
b_st.site = &cb_emlrtRSI;
c_error(&b_st, message(&b_st, y, b_y, &emlrtMCI), &emlrtMCI);
}
if (!obj->isInitialized) {
b_st.site = &cb_emlrtRSI;
if (!obj->isInitialized) {
} else {
c_y = NULL;
m49 = mxCreateCharArray(2, iv200);
for (i = 0; i < 51; i++) {
cv245[i] = cv246[i];
}
emlrtInitCharArrayR2013a(&b_st, 51, m49, cv245);
emlrtAssign(&c_y, m49);
d_y = NULL;
m49 = mxCreateCharArray(2, iv201);
for (i = 0; i < 5; i++) {
cv247[i] = cv248[i];
}
emlrtInitCharArrayR2013a(&b_st, 5, m49, cv247);
emlrtAssign(&d_y, m49);
c_st.site = &cb_emlrtRSI;
c_error(&c_st, message(&c_st, c_y, d_y, &emlrtMCI), &emlrtMCI);
}
c_st.site = &cb_emlrtRSI;
obj->isInitialized = TRUE;
d_st.site = &db_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
memset(&fullGrid[0], 1, sizeof(int8_T) << 6);
for (k = 0; k < 11; k++) {
fullGrid[iv202[k]] = 0;
}
d_st.site = &fs_emlrtRSI;
i = 0;
ii = 1;
exitg2 = FALSE;
while ((exitg2 == FALSE) && (ii < 65)) {
guard2 = FALSE;
if (fullGrid[ii - 1] == 1) {
i++;
ii_data[i - 1] = (int8_T)ii;
if (i >= 64) {
exitg2 = TRUE;
} else {
guard2 = TRUE;
}
} else {
guard2 = TRUE;
}
if (guard2 == TRUE) {
ii++;
}
}
if (1 > i) {
i = 0;
}
for (k = 0; k < i; k++) {
b_ii_data[k] = ii_data[k];
}
for (k = 0; k < i; k++) {
ii_data[k] = b_ii_data[k];
}
d_st.site = &fs_emlrtRSI;
k = obj->pDataLinearIndices->size[0];
obj->pDataLinearIndices->size[0] = i;
emxEnsureCapacity(&d_st, (emxArray__common *)obj->pDataLinearIndices, k,
(int32_T)sizeof(real_T), &pb_emlrtRTEI);
for (k = 0; k < i; k++) {
obj->pDataLinearIndices->data[k] = ii_data[k];
}
c_st.site = &cb_emlrtRSI;
}
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
memcpy(&recv[0], &recv_data[192], sizeof(creal_T) << 6);
emxInit_creal_T(&st, &RLongFirst, 3, &yb_emlrtRTEI, TRUE);
b_st.site = &cb_emlrtRSI;
OFDMDemodulator_stepImpl(&b_st, obj, recv, RLongFirst);
/* First half of long preamble */
st.site = &yr_emlrtRSI;
obj = hPreambleDemod;
if (!obj->isReleased) {
} else {
e_y = NULL;
m49 = mxCreateCharArray(2, iv203);
for (i = 0; i < 45; i++) {
cv241[i] = cv242[i];
}
emlrtInitCharArrayR2013a(&st, 45, m49, cv241);
emlrtAssign(&e_y, m49);
f_y = NULL;
m49 = mxCreateCharArray(2, iv204);
for (i = 0; i < 4; i++) {
cv243[i] = cv244[i];
}
emlrtInitCharArrayR2013a(&st, 4, m49, cv243);
emlrtAssign(&f_y, m49);
b_st.site = &cb_emlrtRSI;
c_error(&b_st, message(&b_st, e_y, f_y, &emlrtMCI), &emlrtMCI);
}
if (!obj->isInitialized) {
b_st.site = &cb_emlrtRSI;
if (!obj->isInitialized) {
} else {
g_y = NULL;
m49 = mxCreateCharArray(2, iv205);
for (i = 0; i < 51; i++) {
cv245[i] = cv246[i];
}
emlrtInitCharArrayR2013a(&b_st, 51, m49, cv245);
emlrtAssign(&g_y, m49);
h_y = NULL;
m49 = mxCreateCharArray(2, iv206);
for (i = 0; i < 5; i++) {
cv247[i] = cv248[i];
}
emlrtInitCharArrayR2013a(&b_st, 5, m49, cv247);
emlrtAssign(&h_y, m49);
c_st.site = &cb_emlrtRSI;
c_error(&c_st, message(&c_st, g_y, h_y, &emlrtMCI), &emlrtMCI);
}
c_st.site = &cb_emlrtRSI;
obj->isInitialized = TRUE;
d_st.site = &db_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
memset(&fullGrid[0], 1, sizeof(int8_T) << 6);
for (k = 0; k < 11; k++) {
fullGrid[iv202[k]] = 0;
}
d_st.site = &fs_emlrtRSI;
i = 0;
ii = 1;
exitg1 = FALSE;
while ((exitg1 == FALSE) && (ii < 65)) {
guard1 = FALSE;
if (fullGrid[ii - 1] == 1) {
i++;
ii_data[i - 1] = (int8_T)ii;
if (i >= 64) {
exitg1 = TRUE;
} else {
guard1 = TRUE;
}
} else {
guard1 = TRUE;
}
if (guard1 == TRUE) {
ii++;
}
}
if (1 > i) {
i = 0;
}
for (k = 0; k < i; k++) {
b_ii_data[k] = ii_data[k];
}
for (k = 0; k < i; k++) {
ii_data[k] = b_ii_data[k];
}
d_st.site = &fs_emlrtRSI;
k = obj->pDataLinearIndices->size[0];
obj->pDataLinearIndices->size[0] = i;
emxEnsureCapacity(&d_st, (emxArray__common *)obj->pDataLinearIndices, k,
(int32_T)sizeof(real_T), &pb_emlrtRTEI);
for (k = 0; k < i; k++) {
obj->pDataLinearIndices->data[k] = ii_data[k];
}
c_st.site = &cb_emlrtRSI;
}
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
memcpy(&b_recv[0], &recv_data[256], sizeof(creal_T) << 6);
emxInit_creal_T(&st, &RLongSecond, 3, &ac_emlrtRTEI, TRUE);
emxInit_creal_T(&st, &b_R, 3, &pb_emlrtRTEI, TRUE);
b_st.site = &cb_emlrtRSI;
OFDMDemodulator_stepImpl(&b_st, obj, b_recv, RLongSecond);
/* Second half of long preamble */
/* %% Preamble Equalization */
/* Get Equalizer tap gains */
k = RLongFirst->size[0];
ii = RLongSecond->size[0];
emlrtDimSizeEqCheckFastR2012b(k, ii, &x_emlrtECI, sp);
st.site = &as_emlrtRSI;
k = b_R->size[0] * b_R->size[1] * b_R->size[2];
b_R->size[0] = RLongFirst->size[0];
b_R->size[1] = 2;
b_R->size[2] = 1;
emxEnsureCapacity(&st, (emxArray__common *)b_R, k, (int32_T)sizeof(creal_T),
&pb_emlrtRTEI);
i = RLongFirst->size[0];
for (k = 0; k < i; k++) {
b_R->data[k] = RLongFirst->data[k];
}
emxFree_creal_T(&RLongFirst);
i = RLongSecond->size[0];
for (k = 0; k < i; k++) {
b_R->data[k + b_R->size[0]] = RLongSecond->data[k];
}
emxFree_creal_T(&RLongSecond);
/* Calculate Equalizer Taps with preamble symbols */
/* Calculate non-normalized channel gains */
for (k = 0; k < 53; k++) {
ii = b_R->size[0];
i = 1 + k;
emlrtDynamicBoundsCheckFastR2012b(i, 1, ii, &lb_emlrtBCI, &st);
}
i = b_R->size[0];
for (k = 0; k < 2; k++) {
for (ii = 0; ii < 53; ii++) {
c_R[ii + 53 * k] = b_R->data[ii + i * k];
}
}
emxFree_creal_T(&b_R);
for (k = 0; k < 53; k++) {
b_tx_longPreamble[k] = tx_longPreamble[k];
b_tx_longPreamble[53 + k] = tx_longPreamble[k];
}
b_st.site = &bt_emlrtRSI;
b_rdivide(c_R, b_tx_longPreamble, RNormal);
/* Known is the original Long Preamble symbols */
/* Scale channel gains */
b_st.site = &ct_emlrtRSI;
d_abs(RNormal, dv13);
memcpy(&dv14[0], &dv13[0], 106U * sizeof(real_T));
b_st.site = &ct_emlrtRSI;
b_power(dv14, dv13);
b_st.site = &ct_emlrtRSI;
c_mean(dv13, REnergy);
b_st.site = &dt_emlrtRSI;
d_mean(RNormal, RConj);
for (k = 0; k < 53; k++) {
RConj[k].im = -RConj[k].im;
b_RConj[k] = RConj[k];
}
b_st.site = &et_emlrtRSI;
c_rdivide(b_RConj, REnergy, RConj);
/* Separate data from preambles */
/* recvData = recv(length(tx.preambles)+1:length(tx.preambles)+(hDataDemod.NumSymbols)*(tx.FFTLength+hDataDemod.CyclicPrefixLength)); */
emlrtVectorVectorIndexCheckR2012b(1280, 1, 1, 960, &w_emlrtECI, sp);
/* CG */
/* OFDM Demod */
st.site = &bs_emlrtRSI;
obj = hDataDemod;
if (!obj->isReleased) {
} else {
i_y = NULL;
m49 = mxCreateCharArray(2, iv207);
for (i = 0; i < 45; i++) {
cv241[i] = cv242[i];
}
emlrtInitCharArrayR2013a(&st, 45, m49, cv241);
emlrtAssign(&i_y, m49);
j_y = NULL;
m49 = mxCreateCharArray(2, iv208);
for (i = 0; i < 4; i++) {
cv243[i] = cv244[i];
}
emlrtInitCharArrayR2013a(&st, 4, m49, cv243);
emlrtAssign(&j_y, m49);
b_st.site = &cb_emlrtRSI;
c_error(&b_st, message(&b_st, i_y, j_y, &emlrtMCI), &emlrtMCI);
}
if (!obj->isInitialized) {
b_st.site = &cb_emlrtRSI;
if (!obj->isInitialized) {
} else {
k_y = NULL;
m49 = mxCreateCharArray(2, iv209);
for (i = 0; i < 51; i++) {
cv245[i] = cv246[i];
}
emlrtInitCharArrayR2013a(&b_st, 51, m49, cv245);
emlrtAssign(&k_y, m49);
l_y = NULL;
m49 = mxCreateCharArray(2, iv210);
for (i = 0; i < 5; i++) {
cv247[i] = cv248[i];
}
emlrtInitCharArrayR2013a(&b_st, 5, m49, cv247);
emlrtAssign(&l_y, m49);
c_st.site = &cb_emlrtRSI;
c_error(&c_st, message(&c_st, k_y, l_y, &emlrtMCI), &emlrtMCI);
}
c_st.site = &cb_emlrtRSI;
g_SystemProp_matlabCodegenSetAn(obj);
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_SystemCore_validateProperties();
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
d_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
memset(&b_fullGrid[0], 1, 768U * sizeof(int8_T));
for (k = 0; k < 12; k++) {
for (ii = 0; ii < 11; ii++) {
b_fullGrid[iv202[ii] + (k << 6)] = 0;
}
b_fullGrid[32 + (k << 6)] = 0;
}
d_st.site = &st_emlrtRSI;
d_st.site = &st_emlrtRSI;
for (k = 0; k < 48; k++) {
b_fullGrid[iv211[k]] = 2;
}
d_st.site = &fs_emlrtRSI;
for (k = 0; k < 768; k++) {
c_fullGrid[k] = (b_fullGrid[k] == 1);
}
eml_find(c_fullGrid, c_ii_data, ii_size);
b_ii_size[0] = ii_size[0];
i = ii_size[0];
for (k = 0; k < i; k++) {
d_ii_data[k] = c_ii_data[k];
}
d_st.site = &fs_emlrtRSI;
h_SystemProp_matlabCodegenSetAn(&d_st, obj, d_ii_data, b_ii_size);
c_st.site = &cb_emlrtRSI;
}
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
c_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_st.site = &cb_emlrtRSI;
b_OFDMDemodulator_stepImpl(SD, &b_st, obj, *(creal_T (*)[960])&recv_data[320],
Rraw, RXPilots);
/* Expand equalizer gains to full frame size */
st.site = &cs_emlrtRSI;
i = 0;
for (ii = 0; ii < 12; ii++) {
ia = 0;
for (k = 0; k < 53; k++) {
preambleGainsFull[i] = RConj[ia];
b_st.site = &ng_emlrtRSI;
ia++;
b_st.site = &og_emlrtRSI;
i++;
}
}
/* Isolate pilot gains from preamble equalizer */
/* Needed to correctly adjust pilot gains */
/* Apply preamble equalizer gains to data and pilots */
/* Correct pilots */
for (i = 0; i < 3; i++) {
iv212[i] = iv213[i];
}
for (k = 0; k < 3; k++) {
b_Rraw[k] = Rraw->size[k];
}
emlrtSizeEqCheckNDR2012b(iv212, b_Rraw, &v_emlrtECI, sp);
/* Correct data */
/* %% Pilot Equalization */
/* Get pilot-based equalizer gains */
st.site = &ds_emlrtRSI;
/* Calculate Equalizer Taps with pilot symbols */
/* Calculate non-normalized channel gains */
b_st.site = &vt_emlrtRSI;
c_st.site = &k_emlrtRSI;
for (k = 0; k < 12; k++) {
for (ii = 0; ii < 4; ii++) {
pilotEqGains_re = preambleGainsFull[((int32_T)
c_tx_pilotLocationsWithoutGuard[ii] + 53 * k) - 1].re * RXPilots[ii + (k
<< 2)].re - preambleGainsFull[((int32_T)
c_tx_pilotLocationsWithoutGuard[ii] + 53 * k) - 1].im * RXPilots[ii + (k
<< 2)].im;
pilotEqGains_im = preambleGainsFull[((int32_T)
c_tx_pilotLocationsWithoutGuard[ii] + 53 * k) - 1].re * RXPilots[ii + (k
<< 2)].im + preambleGainsFull[((int32_T)
c_tx_pilotLocationsWithoutGuard[ii] + 53 * k) - 1].im * RXPilots[ii + (k
<< 2)].re;
if (pilotEqGains_im == 0.0) {
PilotNormal[ii + (k << 2)].re = pilotEqGains_re / tx_pilots[ii + (k << 2)];
PilotNormal[ii + (k << 2)].im = 0.0;
} else if (pilotEqGains_re == 0.0) {
PilotNormal[ii + (k << 2)].re = 0.0;
PilotNormal[ii + (k << 2)].im = pilotEqGains_im / tx_pilots[ii + (k << 2)];
} else {
PilotNormal[ii + (k << 2)].re = pilotEqGains_re / tx_pilots[ii + (k << 2)];
PilotNormal[ii + (k << 2)].im = pilotEqGains_im / tx_pilots[ii + (k << 2)];
}
}
}
/* Scale channel gains */
b_st.site = &wt_emlrtRSI;
for (k = 0; k < 48; k++) {
c_st.site = &qc_emlrtRSI;
d_st.site = &co_emlrtRSI;
a[k] = muDoubleScalarHypot(PilotNormal[k].re, PilotNormal[k].im);
}
b_st.site = &wt_emlrtRSI;
c_st.site = &n_emlrtRSI;
d_st.site = &hp_emlrtRSI;
for (k = 0; k < 48; k++) {
d_st.site = &o_emlrtRSI;
PilotEnergy[k] = a[k] * a[k];
}
b_st.site = &xt_emlrtRSI;
c_st.site = &k_emlrtRSI;
/* Interpolate to data carrier size */
for (k = 0; k < 48; k++) {
if (-PilotNormal[k].im == 0.0) {
b_PilotNormal[k].re = PilotNormal[k].re / PilotEnergy[k];
b_PilotNormal[k].im = 0.0;
} else if (PilotNormal[k].re == 0.0) {
b_PilotNormal[k].re = 0.0;
b_PilotNormal[k].im = -PilotNormal[k].im / PilotEnergy[k];
} else {
b_PilotNormal[k].re = PilotNormal[k].re / PilotEnergy[k];
b_PilotNormal[k].im = -PilotNormal[k].im / PilotEnergy[k];
}
}
b_st.site = &yt_emlrtRSI;
resample(SD, &b_st, b_PilotNormal, pilotEqGains);
/* Apply Equalizer from Pilots */
for (k = 0; k < 12; k++) {
for (ii = 0; ii < 48; ii++) {
preambleGainsFull_data[ii + 48 * k] = preambleGainsFull[((int32_T)
tx_dataSubcarrierIndexies_data[tx_dataSubcarrierIndexies_size[0] * ii] +
53 * k) - 1];
}
}
for (k = 0; k < 12; k++) {
memcpy(&b_preambleGainsFull_data[48 * k], &preambleGainsFull_data[48 * k],
48U * sizeof(creal_T));
}
i = Rraw->size[0];
for (k = 0; k < 12; k++) {
for (ii = 0; ii < 48; ii++) {
c_preambleGainsFull_data[ii + 48 * k].re = b_preambleGainsFull_data[ii +
48 * k].re * Rraw->data[ii + i * k].re - b_preambleGainsFull_data[ii +
48 * k].im * Rraw->data[ii + i * k].im;
c_preambleGainsFull_data[ii + 48 * k].im = b_preambleGainsFull_data[ii +
48 * k].re * Rraw->data[ii + i * k].im + b_preambleGainsFull_data[ii +
48 * k].im * Rraw->data[ii + i * k].re;
}
}
for (k = 0; k < 12; k++) {
for (ii = 0; ii < 48; ii++) {
pilotEqGains_re = pilotEqGains[ii + 48 * k].re;
pilotEqGains_im = pilotEqGains[ii + 48 * k].im;
preambleGainsFull_data_re = c_preambleGainsFull_data[ii + 48 * k].re;
preambleGainsFull_data_im = c_preambleGainsFull_data[ii + 48 * k].im;
R[ii + 48 * k].re = pilotEqGains_re * preambleGainsFull_data_re -
pilotEqGains_im * preambleGainsFull_data_im;
R[ii + 48 * k].im = pilotEqGains_re * preambleGainsFull_data_im +
pilotEqGains_im * preambleGainsFull_data_re;
}
}
/* Save Gains for visualization */
estimate->pilotEqGains.size[0] = 48;
estimate->pilotEqGains.size[1] = 12;
for (k = 0; k < 576; k++) {
estimate->pilotEqGains.data[k] = pilotEqGains[k];
}
estimate->preambleEqGains.size[0] = 53;
for (k = 0; k < 53; k++) {
estimate->preambleEqGains.data[k] = RConj[k];
}
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
int dlassq_(integer *n, doublereal *x, integer *incx,
doublereal *scale, doublereal *sumsq)
{
/* -- LAPACK auxiliary routine (version 3.0) --
Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
Courant Institute, Argonne National Lab, and Rice University
June 30, 1999
Purpose
=======
DLASSQ returns the values scl and smsq such that
( scl**2 )*smsq = x( 1 )**2 +...+ x( n )**2 + ( scale**2 )*sumsq,
where x( i ) = X( 1 + ( i - 1 )*INCX ). The value of sumsq is
assumed to be non-negative and scl returns the value
scl = max( scale, abs( x( i ) ) ).
scale and sumsq must be supplied in SCALE and SUMSQ and
scl and smsq are overwritten on SCALE and SUMSQ respectively.
The routine makes only one pass through the vector x.
Arguments
=========
N (input) INTEGER
The number of elements to be used from the vector X.
X (input) DOUBLE PRECISION array, dimension (N)
The vector for which a scaled sum of squares is computed.
x( i ) = X( 1 + ( i - 1 )*INCX ), 1 <= i <= n.
INCX (input) INTEGER
The increment between successive values of the vector X.
INCX > 0.
SCALE (input/output) DOUBLE PRECISION
On entry, the value scale in the equation above.
On exit, SCALE is overwritten with scl , the scaling factor
for the sum of squares.
SUMSQ (input/output) DOUBLE PRECISION
On entry, the value sumsq in the equation above.
On exit, SUMSQ is overwritten with smsq , the basic sum of
squares from which scl has been factored out.
=====================================================================
Parameter adjustments */
/* System generated locals */
integer i__1, i__2;
doublereal d__1;
/* Local variables */
static doublereal absxi;
static integer ix;
doublereal d_abs(doublereal);
--x;
/* Function Body */
if (*n > 0)
{
i__1 = (*n - 1) * *incx + 1;
i__2 = *incx;
for (ix = 1; i__2 < 0 ? ix >= i__1 : ix <= i__1; ix += i__2)
{
if (x[ix] != 0.)
{
absxi = (d__1 = x[ix], d_abs(d__1));
if (*scale < absxi)
{
/* Computing 2nd power */
d__1 = *scale / absxi;
*sumsq = *sumsq * (d__1 * d__1) + 1;
*scale = absxi;
}
else
{
/* Computing 2nd power */
d__1 = absxi / *scale;
*sumsq += d__1 * d__1;
}
}
/* L10: */
}
}
return 0;
/* End of DLASSQ */
} /* dlassq_ */
void ParticleFilterViewer::paintEvent(QPaintEvent* )
{
QPen pen(Qt::black, 2, Qt::SolidLine);
QPainter painter(this);
double width = this->visibleRegion().boundingRect().width();
double height = this->visibleRegion().boundingRect().height();
double top = 20;
double bottom = height - 20;
double left = 20;
double right = width - 20;
double w_scalar = (bottom - top) / map->width;
double h_scalar = (right - left) / map->height;
//draw map
painter.setPen(pen);
for(int i=0;i<map->parts.size();i++) {
painter.drawLine(left + map->parts[i].v0.x() * w_scalar , top + map->parts[i].v0.y() * h_scalar,
left + map->parts[i].v1.x() * w_scalar, top + map->parts[i].v1.y() * h_scalar );
}
//actual
//robot
int robot_w = 15 * w_scalar;
int robot_h = 10 * h_scalar;
pen = QPen(Qt::black, 2, Qt::SolidLine);
painter.setPen(pen);
painter.save();
painter.translate(left + robot->x * w_scalar, top + robot->y * h_scalar);
painter.rotate(robot->dir * 180 / PI);
painter.drawRect(-robot_w/2, -robot_h/2, robot_w, robot_h);
painter.restore();
//particles
painter.setPen( QPen( Qt::NoPen ) );
for(int i=0;i<particleFilter->pnum;i++) {
painter.setBrush(QBrush(QColor(0,0,0)));
painter.drawEllipse(left + particleFilter->p[i].x * w_scalar, top + particleFilter->p[i].y * h_scalar, 5, 5);
painter.setPen(QPen(QColor(255,0,0)));
//painter.drawLine(left + particleFilter->p[i].x * w_scalar, top + particleFilter->p[i].y * h_scalar, \
left + particleFilter->p[i].p.x() * w_scalar, top + particleFilter->p[i].p.y() * h_scalar);
//painter.drawLine(left + particleFilter->p[i].x * w_scalar, top + particleFilter->p[i].y * h_scalar, \
left + particleFilter->p[i].p1.x() * w_scalar, top + particleFilter->p[i].p1.y() * h_scalar);
}
//best estimate
Particle p = particleFilter->getWieghtedAverage();
painter.setBrush(QBrush(QColor(0,0,255)));
painter.drawEllipse(left + p.x * w_scalar, top + p.y * h_scalar, 5, 5);
painter.setPen(QPen(QColor(0,0,0)));
double error = d_abs(p.x - robot->x) + d_abs(p.y - robot->y) + d_abs(p.dir - robot->dir);
QString status = QString::number(error);
QStaticText status_text(status);
painter.drawStaticText(0,0, status_text);
if(particleFilter->isLocalized()) {
state_error.push_back(error);
double mean = 0;
int total = state_error.size();
for(int i=0;i<total;i++) {
mean += state_error[i];
}
mean /= total;
QString status = "State error:" + QString::number(mean);
QStaticText status_text(status);
painter.drawStaticText(0,20, status_text);
}
}
int dlarfg_(integer *n, doublereal *alpha, doublereal *x,
integer *incx, doublereal *tau)
{
/* -- LAPACK auxiliary routine (version 3.0) --
Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
Courant Institute, Argonne National Lab, and Rice University
September 30, 1994
Purpose
=======
DLARFG generates a real elementary reflector H of order n, such
that
H * ( alpha ) = ( beta ), H' * H = I.
( x ) ( 0 )
where alpha and beta are scalars, and x is an (n-1)-element real
vector. H is represented in the form
H = I - tau * ( 1 ) * ( 1 v' ) ,
( v )
where tau is a real scalar and v is a real (n-1)-element
vector.
If the elements of x are all zero, then tau = 0 and H is taken to be
the unit matrix.
Otherwise 1 <= tau <= 2.
Arguments
=========
N (input) INTEGER
The order of the elementary reflector.
ALPHA (input/output) DOUBLE PRECISION
On entry, the value alpha.
On exit, it is overwritten with the value beta.
X (input/output) DOUBLE PRECISION array, dimension
(1+(N-2)*abs(INCX))
On entry, the vector x.
On exit, it is overwritten with the vector v.
INCX (input) INTEGER
The increment between elements of X. INCX > 0.
TAU (output) DOUBLE PRECISION
The value tau.
=====================================================================
Parameter adjustments */
/* System generated locals */
integer i__1;
doublereal d__1;
/* Builtin functions */
doublereal d_sign(doublereal *, doublereal *);
doublereal d_abs(doublereal);
/* Local variables */
static doublereal beta;
extern doublereal dnrm2_(integer *, doublereal *, integer *);
static integer j;
extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *,
integer *);
static doublereal xnorm;
extern doublereal dlapy2_(doublereal *, doublereal *), dlamch_(char *);
static doublereal safmin, rsafmn;
static integer knt;
--x;
/* Function Body */
if (*n <= 1)
{
*tau = 0.;
return 0;
}
i__1 = *n - 1;
xnorm = dnrm2_(&i__1, &x[1], incx);
if (xnorm == 0.)
{
/* H = I */
*tau = 0.;
}
else
{
/* general case */
d__1 = dlapy2_(alpha, &xnorm);
beta = -d_sign(&d__1, alpha);
safmin = dlamch_("S") / dlamch_("E");
if (d_abs(beta) < safmin)
{
/* XNORM, BETA may be inaccurate; scale X and recompute them */
rsafmn = 1. / safmin;
knt = 0;
L10:
++knt;
i__1 = *n - 1;
dscal_(&i__1, &rsafmn, &x[1], incx);
beta *= rsafmn;
*alpha *= rsafmn;
if (d_abs(beta) < safmin)
{
goto L10;
}
/* New BETA is at most 1, at least SAFMIN */
i__1 = *n - 1;
xnorm = dnrm2_(&i__1, &x[1], incx);
d__1 = dlapy2_(alpha, &xnorm);
beta = -d_sign(&d__1, alpha);
*tau = (beta - *alpha) / beta;
i__1 = *n - 1;
d__1 = 1. / (*alpha - beta);
dscal_(&i__1, &d__1, &x[1], incx);
/* If ALPHA is subnormal, it may lose relative accuracy */
*alpha = beta;
i__1 = knt;
for (j = 1; j <= i__1; ++j)
{
*alpha *= safmin;
/* L20: */
}
}
else
{
*tau = (beta - *alpha) / beta;
i__1 = *n - 1;
d__1 = 1. / (*alpha - beta);
dscal_(&i__1, &d__1, &x[1], incx);
*alpha = beta;
}
}
return 0;
/* End of DLARFG */
} /* dlarfg_ */
void update(){
// set fake walk/rotation values
fake_walk = walk;
if(fake_walk > 0){ // we assume that the robot understands that it is still, when it is
fake_walk -= max_walk_error;
fake_walk += ((double)(rand())/(double)(RAND_MAX)) * 2 * max_walk_error;
if(fake_walk < 0){fake_walk = 0;}
}
fake_rotation = rotation;
fake_rotation -= max_rotation_error;
fake_rotation += ((double)(rand())/(double)(RAND_MAX)) * 2 * max_rotation_error;
// update robot position and rotation
if(walk > 0){
// modify robot position
current_pose.x = current_pose.x + cos(current_pose.theta) * walk;
current_pose.y = current_pose.y + sin(current_pose.theta) * walk;
current_pose.theta += rotation;
current_pose.theta = normalizeAngle(current_pose.theta);
// modify robot position guess
fake_current_pose.x = fake_current_pose.x + cos(fake_current_pose.theta) * fake_walk;
fake_current_pose.y = fake_current_pose.y + sin(fake_current_pose.theta) * fake_walk;
fake_current_pose.theta += fake_rotation;
fake_current_pose.theta = normalizeAngle(fake_current_pose.theta);
move_amount += walk;
rotation_amount += d_abs(rotation);
}
// check capture triggers
if(move_amount > move_trigger || rotation_amount > rotation_trigger){
// std::cout << std::endl;
// std::cout << "moved: " << move_amount << "; rotated: " << rotation_amount << "; ";
// if (move_amount > move_trigger){
// std::cout << "move trigger; ";
// }
// if (rotation_amount > rotation_trigger){
// std::cout << "rotation trigger; ";
// }
// std::cout << std::endl;
move_amount = 0;
rotation_amount = 0;
time_to_capture = true;
}
// eventually capture
if(time_to_capture){
// reset time_to_capture
time_to_capture = false;
// detect in-range landmarks
for(unsigned int i=0; i<landmarks.size(); i++){
// compute the distance between the landmark and the robot
double dx = landmarks[i].x - current_pose.x;
double dy = landmarks[i].y - current_pose.y;
double distance = sqrt(pow(dx,2) + pow(dy,2));
// check if the landmark is within the range
if(distance < range){
detected_landmarks.push_back(&(landmarks[i]));
perceptions.push_back(computeAngle(&landmarks[i], ¤t_pose));
}
}
// compute transformation matrix between last capture pose and current pose (and do the same for the robot guess)
Eigen::Matrix3d m1 = r2m(last_captured_pose);
Eigen::Matrix3d m2 = r2m(current_pose);
Eigen::Matrix3d fm1 = r2m(fake_last_captured_pose);
Eigen::Matrix3d fm2 = r2m(fake_current_pose);
RobotPose transform = m2r(m1.inverse()*m2);
RobotPose fake_transform = m2r(fm1.inverse()*fm2);
// write stuff on files
truth_out << transform.x << " " << transform.y << " " << transform.theta;
noised_out << fake_transform.x << " " << fake_transform.y << " " << fake_transform.theta;
for(unsigned int i = 0; i<perceptions.size(); i++){
truth_out << " " << perceptions[i];
noised_out << " " << perceptions[i] - max_perception_error + 2 * max_perception_error * ((double)rand())/((double)RAND_MAX);
}
truth_out << std::endl;
noised_out << std::endl;
// clear the vectors
detected_landmarks.clear();
perceptions.clear();
// update last_captured_pose
last_captured_pose = current_pose;
fake_last_captured_pose = fake_current_pose;
}
}
/* Subroutine */ int dlasv2_(doublereal *f, doublereal *g, doublereal *h__,
doublereal *ssmin, doublereal *ssmax, doublereal *snr, doublereal *
csr, doublereal *snl, doublereal *csl)
{
/* -- LAPACK auxiliary routine (version 3.0) --
Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
Courant Institute, Argonne National Lab, and Rice University
October 31, 1992
Purpose
=======
DLASV2 computes the singular value decomposition of a 2-by-2
triangular matrix
[ F G ]
[ 0 H ].
On return, abs(SSMAX) is the larger singular value, abs(SSMIN) is the
smaller singular value, and (CSL,SNL) and (CSR,SNR) are the left and
right singular vectors for abs(SSMAX), giving the decomposition
[ CSL SNL ] [ F G ] [ CSR -SNR ] = [ SSMAX 0 ]
[-SNL CSL ] [ 0 H ] [ SNR CSR ] [ 0 SSMIN ].
Arguments
=========
F (input) DOUBLE PRECISION
The (1,1) element of the 2-by-2 matrix.
G (input) DOUBLE PRECISION
The (1,2) element of the 2-by-2 matrix.
H (input) DOUBLE PRECISION
The (2,2) element of the 2-by-2 matrix.
SSMIN (output) DOUBLE PRECISION
abs(SSMIN) is the smaller singular value.
SSMAX (output) DOUBLE PRECISION
abs(SSMAX) is the larger singular value.
SNL (output) DOUBLE PRECISION
CSL (output) DOUBLE PRECISION
The vector (CSL, SNL) is a unit left singular vector for the
singular value abs(SSMAX).
SNR (output) DOUBLE PRECISION
CSR (output) DOUBLE PRECISION
The vector (CSR, SNR) is a unit right singular vector for the
singular value abs(SSMAX).
Further Details
===============
Any input parameter may be aliased with any output parameter.
Barring over/underflow and assuming a guard digit in subtraction, all
output quantities are correct to within a few units in the last
place (ulps).
In IEEE arithmetic, the code works correctly if one matrix element is
infinite.
Overflow will not occur unless the largest singular value itself
overflows or is within a few ulps of overflow. (On machines with
partial overflow, like the Cray, overflow may occur if the largest
singular value is within a factor of 2 of overflow.)
Underflow is harmless if underflow is gradual. Otherwise, results
may correspond to a matrix modified by perturbations of size near
the underflow threshold.
===================================================================== */
/* Table of constant values */
static doublereal c_b3 = 2.;
static doublereal c_b4 = 1.;
doublereal AuxRes;
/* System generated locals */
doublereal d__1;
/* Builtin functions */
doublereal d_sqrt(doublereal *), d_sign(doublereal *, doublereal *);
doublereal d_abs(doublereal);
/* Local variables */
static integer pmax;
static doublereal temp;
static logical swap;
static doublereal a, d__, l, m, r__, s, t, tsign, fa, ga, ha;
extern doublereal dlamch_(char *);
static doublereal ft, gt, ht, mm;
static logical gasmal;
static doublereal tt, clt, crt, slt, srt;
ft = *f;
fa = d_abs(ft);
ht = *h__;
ha = d_abs(*h__);
/* PMAX points to the maximum absolute element of matrix
PMAX = 1 if F largest in absolute values
PMAX = 2 if G largest in absolute values
PMAX = 3 if H largest in absolute values */
pmax = 1;
swap = ha > fa;
if (swap) {
pmax = 3;
temp = ft;
ft = ht;
ht = temp;
temp = fa;
fa = ha;
ha = temp;
/* Now FA .ge. HA */
}
gt = *g;
ga = d_abs(gt);
if (ga == 0.) {
/* Diagonal matrix */
*ssmin = ha;
*ssmax = fa;
clt = 1.;
crt = 1.;
slt = 0.;
srt = 0.;
} else {
gasmal = TRUE_;
if (ga > fa) {
pmax = 2;
if (fa / ga < dlamch_("EPS")) {
/* Case of very large GA */
gasmal = FALSE_;
*ssmax = ga;
if (ha > 1.) {
*ssmin = fa / (ga / ha);
} else {
*ssmin = fa / ga * ha;
}
clt = 1.;
slt = ht / gt;
srt = 1.;
crt = ft / gt;
}
}
if (gasmal) {
/* Normal case */
d__ = fa - ha;
if (d__ == fa) {
/* Copes with infinite F or H */
l = 1.;
} else {
l = d__ / fa;
}
/* Note that 0 .le. L .le. 1 */
m = gt / ft;
/* Note that abs(M) .le. 1/macheps */
t = 2. - l;
/* Note that T .ge. 1 */
mm = m * m;
tt = t * t;
AuxRes = tt + mm;
s = d_sqrt(&AuxRes);
/* Note that 1 .le. S .le. 1 + 1/macheps */
if (l == 0.) {
r__ = d_abs(m);
} else
{
AuxRes= l * l + mm;
r__ = d_sqrt(&AuxRes);
}
/* Note that 0 .le. R .le. 1 + 1/macheps */
a = (s + r__) * .5;
/* Note that 1 .le. A .le. 1 + abs(M) */
*ssmin = ha / a;
*ssmax = fa * a;
if (mm == 0.) {
/* Note that M is very tiny */
if (l == 0.) {
t = d_sign(&c_b3, &ft) * d_sign(&c_b4, >);
} else {
t = gt / d_sign(&d__, &ft) + m / t;
}
} else {
t = (m / (s + t) + m / (r__ + l)) * (a + 1.);
}
AuxRes = t*t + 4.;
l = d_sqrt(&AuxRes);
crt = 2. / l;
srt = t / l;
clt = (crt + srt * m) / a;
slt = ht / ft * srt / a;
}
}
if (swap) {
*csl = srt;
*snl = crt;
*csr = slt;
*snr = clt;
} else {
*csl = clt;
*snl = slt;
*csr = crt;
*snr = srt;
}
/* Correct signs of SSMAX and SSMIN */
if (pmax == 1) {
tsign = d_sign(&c_b4, csr) * d_sign(&c_b4, csl) * d_sign(&c_b4, f);
}
if (pmax == 2) {
tsign = d_sign(&c_b4, snr) * d_sign(&c_b4, csl) * d_sign(&c_b4, g);
}
if (pmax == 3) {
tsign = d_sign(&c_b4, snr) * d_sign(&c_b4, snl) * d_sign(&c_b4, h__);
}
*ssmax = d_sign(ssmax, &tsign);
d__1 = tsign * d_sign(&c_b4, f) * d_sign(&c_b4, h__);
*ssmin = d_sign(ssmin, &d__1);
return 0;
/* End of DLASV2 */
} /* dlasv2_ */
