bool test_file_directory_find_invalid_argument (Test *test)
{
char *path;
Directory *directory;
TITLE ();
CATCH (!(path = directory_current_path ()));
CATCH (!string_append (&path, "/stage/find"));
/*
d stage/find
f f1
d stage/find/d1
*/
CATCH (!(directory = directory_open (path)));
string_destroy (path);
CATCH (directory_find_file (NULL, "file"));
CATCH (error_count () != 1);
CATCH (error_at (0).error != ErrorInvalidArgument);
error_reset ();
CATCH (directory_find_directory (NULL, "directory"));
CATCH (error_count () != 1);
CATCH (error_at (0).error != ErrorInvalidArgument);
error_reset ();
CATCH (directory_find_file (directory, NULL));
CATCH (error_count () != 1);
CATCH (error_at (0).error != ErrorInvalidArgument);
error_reset ();
CATCH (directory_find_directory (directory, NULL));
CATCH (error_count () != 1);
CATCH (error_at (0).error != ErrorInvalidArgument);
error_reset ();
directory_close (directory);
PASS ();
}
} END_TEST
/**
* \brief Test function invocation on not created group list.
*
* This test must be run without setup funtions which are used for other tests
*
* Its aim is to test response of function when GroupList is not allocated.
*/
START_TEST(test_noexistent) {
ImageGroup *tmp = group_create("test", 'a');
fail_unless( FAIL == glist_group_insert(tmp), NULL);
fail_unless( FAIL == glist_group_remove_by_index(0), NULL);
fail_unless( FAIL == glist_get_by_index(0), NULL);
fail_unless( NULL == glist_get_by_tag('Y'), NULL);
fail_unless( NULL == glist_get_by_name("some name"), NULL);
fail_unless( -1 == glist_get_size(), NULL);
group_destroy(tmp);
error_reset();
} END_TEST
const spatialdb_t *spatialdb_detect_schema(sqlite3 *db) {
char message_buffer[256];
errorstream_t error;
error_init_fixed(&error, message_buffer, 256);
const spatialdb_t *schemas[] = {
spatialdb_geopackage_schema(),
spatialdb_spatialite4_schema(),
spatialdb_spatialite3_schema(),
spatialdb_spatialite2_schema(),
NULL
};
const spatialdb_t **schema = &schemas[0];
while (*schema != NULL) {
error_reset(&error);
(*schema)->check_meta(db, "main", SQL_CHECK_PRIMARY_KEY | SQL_CHECK_NULLABLE, &error);
if (error_count(&error) == 0) {
break;
} else {
schema++;
}
}
if (*schema != NULL) {
return *schema;
} else {
return schemas[0];
}
}
} END_TEST
/**
* \brief Test to create application view.
*
* The test create and destroy application view. Check if everything behaves
* correctly.
*/
START_TEST(test_create_application) {
ViewGeneric *view = NULL;
view = view_create(VIEW_APPLICATION);
fail_if( NULL == view, NULL );
fail_if( NULL == view->specific_data, NULL );
fail_if( NULL != view->model, NULL );
fail_if( NULL != view->controller, NULL );
fail_if( VIEW_APPLICATION != view->type, NULL );
fail_if( 0 != view->last_sequence, NULL );
fail_if( 1 != view->reference, NULL );
// check that simple destruction won't work
fail_if( OK == view_destroy(view), NULL );
fail_if( ! error_is_error(), NULL );
error_reset();
// check that unreference work
fail_if( OK != view_unref(view), NULL );
} END_TEST
tParams * parser_parse(char * buff) {
error_reset();
parser_generate_args(buff);
if(!error_found()) {
params.args_amount = args_amount;
params.args = args;
return ¶ms;
} else
return NULL;
}
bool test_object_id_set_counter (Test *test)
{
TITLE ();
object_id_set_counter (123);
CATCH (object_id () != 123);
object_id_set_counter (0);
CATCH (object_id () != 0);
object_id_set_counter (ULLONG_MAX);
CATCH (object_id () != ULLONG_MAX);
error_reset ();
PASS ();
}
/**
* The function reset error buffer after the test.
*/
static void test_generic_teardown(void)
{
COLOR_TEARDOWN_START;
fail_unless( OK == view_unref(view_generic), NULL);
fail_unless( OK == model_unref(model_generic1), NULL);
fail_unless( OK == model_unref(model_generic2), NULL);
fail_unless( OK == cntl_unref(cntl_generic1), NULL);
fail_unless( OK == cntl_unref(cntl_generic2), NULL);
error_reset();
COLOR_TEARDOWN_END;
return ;
}
} END_TEST
/**
* Test removal of pointers from GroupList
*/
START_TEST(test_group_remove) {
// fill some data into list
fail_unless( OK == glist_group_insert(group_data[0]), NULL);
fail_unless( OK == glist_group_insert(group_data[1]), NULL);
fail_unless( OK == glist_group_insert(group_data[2]), NULL);
fail_unless( OK == glist_group_insert(group_data[3]), NULL);
fail_unless( OK == glist_group_insert(group_data[4]), NULL);
fail_unless( 5 == glist_get_size(), NULL);
// simple removal of present element
fail_unless( OK == glist_group_remove_by_index(1), NULL );
fail_unless( 'a' == glist_get_by_index(0)->tag, NULL);
fail_unless( 'e' == glist_get_by_index(1)->tag, NULL);
fail_unless( 'c' == glist_get_by_index(2)->tag, NULL);
fail_unless( 'd' == glist_get_by_index(3)->tag, NULL);
fail_unless( 4 == glist_get_size(), NULL);
// removal of image group by tag
fail_unless( OK == glist_group_remove_by_tag('e'), NULL );
fail_unless( 'a' == glist_get_by_index(0)->tag, NULL);
fail_unless( 'd' == glist_get_by_index(1)->tag, NULL);
fail_unless( 'c' == glist_get_by_index(2)->tag, NULL);
fail_unless( 3 == glist_get_size(), NULL);
// removal of image group by name
fail_unless( OK == glist_group_remove_by_name("test group 4"), NULL );
fail_unless( 'a' == glist_get_by_index(0)->tag, NULL);
fail_unless( 'c' == glist_get_by_index(1)->tag, NULL);
fail_unless( 2 == glist_get_size(), NULL);
// removal of non-existent tag or name should not fail
fail_unless( OK == glist_group_remove_by_name("test group 4"), NULL );
fail_unless( OK == glist_group_remove_by_tag('e'), NULL );
// removal of out of bounds index
fail_unless( FAIL == glist_group_remove_by_index(-1), NULL);
fail_unless( FAIL == glist_group_remove_by_index(4), NULL);
fail_unless( 2 == glist_get_size(), NULL);
error_reset();
} END_TEST
adv_error mouseb_init(void)
{
unsigned i;
assert(mouseb_state.driver_current == 0);
assert(!mouseb_state.is_active_flag);
if (!mouseb_state.is_initialized_flag) {
mouseb_default();
}
/* store the error prefix */
error_nolog_set("Unable to initialize the mouse driver. The errors are:\n");
for (i = 0; i < mouseb_state.driver_mac; ++i) {
const adv_device* dev;
dev = device_match(mouseb_state.name, (const adv_driver*)mouseb_state.driver_map[i], 1);
error_cat_set(mouseb_state.driver_map[i]->name, 1);
if (dev && mouseb_state.driver_map[i]->init(dev->id) == 0) {
mouseb_state.driver_current = mouseb_state.driver_map[i];
break;
}
}
error_cat_set(0, 0);
if (!mouseb_state.driver_current)
return -1;
error_reset();
log_std(("mouseb: select driver %s\n", mouseb_state.driver_current->name));
mouseb_state.is_active_flag = 1;
mouseb_state.is_enabled_flag = 0;
for (i = 0; i < mouseb_count_get(); ++i) {
char name[DEVICE_NAME_MAX];
if (mouseb_device_name_get(i, name) != 0)
strcpy(name, DEVICE_NONAME);
log_std(("mouseb: identifier %u '%s'\n", i, name));
}
return 0;
}
adv_error keyb_init(adv_bool disable_special)
{
unsigned i;
assert(keyb_state.driver_current == 0);
assert(!keyb_state.is_active_flag);
if (!keyb_state.is_initialized_flag) {
keyb_default();
}
/* store the error prefix */
error_nolog_set("Unable to initialize the keyboard driver. The errors are:\n");
for (i = 0; i < keyb_state.driver_mac; ++i) {
const adv_device* dev;
dev = device_match(keyb_state.name, (const adv_driver*)keyb_state.driver_map[i], 0);
error_cat_set(keyb_state.driver_map[i]->name, 1);
if (dev && keyb_state.driver_map[i]->init(dev->id, disable_special) == 0) {
keyb_state.driver_current = keyb_state.driver_map[i];
break;
}
}
error_cat_set(0, 0);
if (!keyb_state.driver_current)
return -1;
error_reset();
log_std(("keyb: select driver %s\n", keyb_state.driver_current->name));
keyb_state.is_active_flag = 1;
keyb_state.is_enabled_flag = 0;
for (i = 0; i < keyb_count_get(); ++i) {
char name[DEVICE_NAME_MAX];
if (keyb_device_name_get(i, name) != 0)
strcpy(name, DEVICE_NONAME);
log_std(("keyb: identifier %u '%s'\n", i, name));
}
return 0;
}
} END_TEST
/**
* The function reset error buffer before the test.
*/
static void test_generic_setup(void)
{
COLOR_SETUP_START;
view_generic = view_create(VIEW_NONE);
model_generic1 = model_create(MODEL_NONE);
model_generic2 = model_create(MODEL_NONE);
cntl_generic1 = cntl_create(CNTL_NONE);
cntl_generic2 = cntl_create(CNTL_NONE);
error_reset();
COLOR_SETUP_END;
return ;
}
static void start_connecting(struct connector *c)
{
for (;;) {
struct addrinfo *ai = c->next_addrinfo;
if (!ai)
break;
/* If we have an existing connecting socket, dispose of it. */
connector_close(c);
c->next_addrinfo = ai->ai_next;
error_reset(&c->err);
c->fd = make_socket(ai->ai_family, ai->ai_socktype, &c->err);
if (c->fd < 0)
continue;
c->watched_fd
= watched_fd_create(c->fd, connector_handle_events, c);
if (connect(c->fd, ai->ai_addr, ai->ai_addrlen) >= 0) {
/* Immediately connected. Not sure this can
actually happen. */
wait_list_up(&c->connecting, 1);
return;
}
else if (would_block()) {
/* Writeability will indicate that the connection has
* been established. */
if (!watched_fd_set_interest(c->watched_fd,
WATCHED_FD_OUT, &c->err))
/* Give up and propagate the error */
break;
return;
}
else {
error_errno(&c->err, "connect");
}
}
/* Ran out of addresses to try, so we are done. We should have
an error to report. */
assert(!error_ok(&c->err));
simple_socket_wake_all(&c->socket->base);
}
bool test_file_open_fail (Test *test)
{
Directory *directory;
File *file;
char *path;
TITLE ();
CATCH (!(path = directory_current_path ()));
CATCH (!string_append (&path, "/stage/open_fail"));
/* stage does not exist */
CATCH (!(directory = directory_open (path)));
string_destroy (path);
CATCH (!directory_read (directory));
CATCH (!(file = directory_find_file (directory, "f1")));
CATCH (file_open (file));
CATCH (error_count () != 1);
CATCH (error_at (0).error != ErrorSystemCall);
error_reset ();
CATCH (file_open (NULL));
CATCH (error_count () != 1);
CATCH (error_at (0).error != ErrorInvalidArgument);
directory_close (directory);
PASS ();
}
// count strings x, their prefixes or suffixes
// and return counts greater than lower.
//
// note that for string counting the number of
// nodes usually will be much greater than the
// number of result strings. however, we could
// not determine a better upper bound without
// traversing the tree.
SEXP R_utf8CountString(SEXP x, SEXP R_n, SEXP R_lower, SEXP R_type,
SEXP R_verbose, SEXP R_persistent, SEXP R_useBytes) {
if (!persistent && rpn) {
cpnfree(rpn);
rpn = 0;
warning("cleaning up stale state");
}
if (isNull(x) || TYPEOF(x) != VECSXP)
error("'x' not of type list");
if (isNull(R_n) || TYPEOF(R_n) != INTSXP)
error("'n' not of type integer");
if (isNull(R_lower) || TYPEOF(R_lower) != INTSXP)
error("'lower' not of type integer");
if (isNull(R_type) || TYPEOF(R_type) != INTSXP)
error("'type' not of type integer");
if (isNull(R_verbose) || TYPEOF(R_verbose) != LGLSXP)
error("'verbose' not of type logical");
if (isNull(R_persistent) || TYPEOF(R_persistent) != LGLSXP)
error("'persistent' not of type logical");
if (isNull(R_useBytes) || TYPEOF(R_useBytes) != LGLSXP)
error("'useBytes' not of type logical");
long l, n = 0;
int h, i, j, k, type;
const unsigned char *c;
SEXP r, s;
if (!persistent) {
known_to_be_utf8 = utf8locale();
known_to_be_latin1 = latin1locale();
use_bytes = *LOGICAL(R_useBytes);
} else
if (use_bytes != *LOGICAL(R_useBytes))
error("change of useBytes in persistent mode");
else
if (known_to_be_utf8 != utf8locale() ||
known_to_be_latin1 != latin1locale())
error_reset("change of locale in persistent mode");
persistent = LOGICAL(R_persistent)[0];
if (!persistent) {
tcnt = INTEGER(R_lower)[0];
if (tcnt < 0)
error_reset("'lower' invalid value");
}
type = INTEGER(R_type)[0];
switch (type) {
case 0: // strings
inc = 0;
break;
case 1: // prefixes
case 2: // suffixes
case 3:
n = INTEGER(R_n)[0];
if (n < 0)
error_reset("'n' invalid value");
if (n == 0)
return R_NilValue;
inc = 1;
break;
default:
error_reset("'type' invalid value");
}
#ifdef _TIME_H
clock_t t2, t1, t0 = clock();
if (LOGICAL(R_verbose)[0] == TRUE) {
Rprintf("counting ...");
#ifdef __DEBUG
Rprintf("\n");
#endif
}
#endif
nap = 0;
for (i = 0; i < LENGTH(x); i++) {
r = VECTOR_ELT(x, i);
if (TYPEOF(r) != STRSXP)
error_reset("not of type character");
for (j = 0; j < LENGTH(r); j++) {
s = STRING_ELT(r, j);
l = LENGTH(s);
if (s == NA_STRING || !l)
continue;
#ifdef __TRANSLATE
if (!use_bytes) {
c = (const unsigned char *) translateChar(s);
l = strlen((const char *) c);
} else
c = (const unsigned char *) CHAR(s);
#else
c = (const unsigned char *) CHAR(s);
#endif
if (!use_bytes &&
known_to_be_utf8 &&
tau_pcre_valid_utf8(c, l) >= 0)
error_reset("not a valid UTF-8 string");
if (type > 1) {
if (reverse_copy_utf8(c, l, n) >= 0)
error_reset("cannot copy string to buffer");
} else {
if (type < 1)
n = l;
h = 0;
for (k = 0; k < l; k++) {
if (c[k] == '\0')
continue;
if (k < __CBUF_SIZE - 1)
cbuf[k] = c[k];
else
error_reset("cannot copy string to buffer");
if (use_bytes ||
!known_to_be_utf8 ||
(c[k] & 0xC0) != 0x80) {
h++;
if (h > n)
break;
}
}
cbuf[k] = 0;
}
#ifdef __DEBUG
Rprintf(" %s\n", cbuf);
#endif
h = nap + 1;
lpn = 0;
rpn = cpncount(rpn, cbuf);
if (nap != h)
error_reset("cannot add string to tree");
if (!inc) {
if (lpn) // should never be NULL
lpn->count++;
}
}
R_CheckUserInterrupt();
}
#ifdef _TIME_H
t1 = clock();
if (LOGICAL(R_verbose)[0] == TRUE) {
Rprintf(" %i string(s) using %i nodes [%.2fs]\n", nap, ncpn,
((double) t1 - t0) / CLOCKS_PER_SEC);
if (!persistent)
Rprintf("writing ...");
#ifdef __DEBUG
Rprintf("\n");
#endif
}
#endif
if (persistent)
return R_NilValue;
nap = enc = 0;
rval = PROTECT(allocVector(INTSXP, ncpn));
setAttrib(rval, R_NamesSymbol, (nval = allocVector(STRSXP, ncpn)));
cpnretprefix(rpn, 0);
if (ncpn) {
cpnfree(rpn);
rpn = 0;
error_reset("cannot retrieve count(s)");
}
rpn = 0;
// reverse the reversed strings
if (type == 2)
for (i = 0; i < nap; i++) {
s = STRING_ELT(nval, i);
reverse_copy_utf8((const unsigned char *) CHAR(s), LENGTH(s), -1);
SET_STRING_ELT(nval, i, mkCharCE((const char *) cbuf,
getCharCE(s)));
}
#ifdef _TIME_H
t2 = clock();
if (LOGICAL(R_verbose)[0] == TRUE)
Rprintf(" %i strings [%.2fs]\n", nap,
((double) t2 - t1) / CLOCKS_PER_SEC);
#endif
// reduce
if (nap < LENGTH(rval)) {
r = PROTECT(allocVector(INTSXP, nap));
setAttrib(r, R_NamesSymbol, (s = allocVector(STRSXP, nap)));
while (nap-- > 0) {
INTEGER(r)[nap] = INTEGER(rval)[nap];
SET_STRING_ELT(s, nap, STRING_ELT(nval, nap));
}
UNPROTECT(2);
return r;
}
UNPROTECT(1);
return rval;
}
SEXP R_utf8CountNgram(SEXP x, SEXP R_n, SEXP R_lower, SEXP R_verbose,
SEXP R_persistent, SEXP R_useBytes) {
if (!persistent && rpn) {
cpnfree(rpn);
rpn = 0;
warning("cleaning up stale state");
}
if (isNull(x) || TYPEOF(x) != VECSXP)
error("'x' not of type list");
if (isNull(R_n) || TYPEOF(R_n) != INTSXP)
error("'n' not of type integer");
if (isNull(R_lower) || TYPEOF(R_lower) != INTSXP)
error("'lower' not of type integer");
if (isNull(R_verbose) || TYPEOF(R_verbose) != LGLSXP)
error("'verbose' not of type logical");
if (isNull(R_persistent) || TYPEOF(R_persistent) != LGLSXP)
error("'persistent' not of type logical");
if (isNull(R_useBytes) || TYPEOF(R_useBytes) != LGLSXP)
error("'useBytes' not of type logical");
long l;
int h, i, j, k, m, n;
const unsigned char *c;
SEXP r, s;
if (!persistent) {
known_to_be_utf8 = utf8locale();
known_to_be_latin1 = latin1locale();
use_bytes = *LOGICAL(R_useBytes);
} else
if (use_bytes != *LOGICAL(R_useBytes))
error("change of useBytes in persistent mode");
else
if (known_to_be_utf8 != utf8locale() ||
known_to_be_latin1 != latin1locale())
error_reset("change of locale in persistent mode");
persistent = LOGICAL(R_persistent)[0];
n = INTEGER(R_n)[0];
if (n < 0)
error_reset("'n' invalid value");
if (n == 0)
return R_NilValue;
if (!persistent) {
tcnt = INTEGER(R_lower)[0];
if (tcnt < 0)
error_reset("'lowr' invalid value");
}
#ifdef _TIME_H
clock_t t2, t1, t0 = clock();
if (LOGICAL(R_verbose)[0] == TRUE) {
Rprintf("counting ...");
#ifdef __DEBUG
Rprintf("\n");
#endif
}
#endif
nap = 0;
inc = 1;
for (i = 0; i < LENGTH(x); i++) {
r = VECTOR_ELT(x, i);
if (TYPEOF(r) != STRSXP)
error_reset("not of type character");
for (j = 0; j < LENGTH(r); j++) {
s = STRING_ELT(r, j);
l = LENGTH(s);
if (s == NA_STRING || !l)
continue;
#ifdef __TRANSLATE
if (!use_bytes) {
c = (const unsigned char *) translateChar(s);
l = strlen((const char *) c);
} else
c = (const unsigned char *) CHAR(s);
#else
c = (const unsigned char *) CHAR(s);
#endif
// strings of unknown encoding are not translated
// or strings marked as UTF-8 could be invalid, so
// we have to check.
if (!use_bytes &&
known_to_be_utf8 &&
tau_pcre_valid_utf8(c, l) >= 0)
error_reset("not a valid UTF-8 string");
/* in an UTF-8 multibyte sequence any byte
* except the first has 10 as its leading bits.
* thus, 1) the byte cannot be the start of a
* suffix and 2) we have to expand the current
* window.
*
* '\1' is a special boundary marker that triggers
* reduced counting, ie omission of windows which
* start at a boundary and shrinkage of windows
* which end at a boundary.
*/
int b;
for (k = 0; k < l; k++) {
if (c[k] == '\0')
continue;
if (!use_bytes &&
known_to_be_utf8 &&
(c[k] & 0xC0) == 0x80)
continue;
if (k == 1 && c[0] == '\1')
continue;
h = 0;
m = k;
b = k;
while (m < l) {
if (m-k < __CBUF_SIZE)
cbuf[m-k] = c[m];
else
error_reset("cannot copy string to buffer");
if (use_bytes ||
!known_to_be_utf8 ||
(c[m] & 0xC0) != 0x80) {
h++;
if (h > n) {
h--;
if (c[m] == '\1') {
h--;
m = b;
}
break;
}
b = m;
}
m++;
}
cbuf[m-k] = 0;
#ifdef __DEBUG
Rprintf(" %i %i %i %s\n", k+1, m, h, cbuf);
#endif
h = nap + 1;
rpn = cpncount(rpn, cbuf);
if (nap != h)
error_reset("cannot add string to tree");
}
}
R_CheckUserInterrupt();
}
#ifdef _TIME_H
t1 = clock();
if (LOGICAL(R_verbose)[0] == TRUE) {
Rprintf(" %i string(s) using %i nodes [%.2fs]\n", nap, ncpn,
((double) t1 - t0) / CLOCKS_PER_SEC);
if (!persistent)
Rprintf("writing ...");
#ifdef __DEBUG
Rprintf("\n");
#endif
}
#endif
if (persistent)
return R_NilValue;
nap = enc = 0;
rval = PROTECT(allocVector(INTSXP, ncpn));
setAttrib(rval, R_NamesSymbol, (nval = allocVector(STRSXP, ncpn)));
cpnretprefix(rpn, 0);
if (ncpn) {
cpnfree(rpn);
rpn = 0;
error("cannot retrieve count(s)");
}
rpn = 0;
#ifdef _TIME_H
t2 = clock();
if (LOGICAL(R_verbose)[0] == TRUE)
Rprintf(" %i strings [%.2fs]\n", nap,
((double) t2 - t1) / CLOCKS_PER_SEC);
#endif
// reduce
if (nap < LENGTH(rval)) {
r = PROTECT(allocVector(INTSXP, nap));
setAttrib(r, R_NamesSymbol, (s = allocVector(STRSXP, nap)));
while (nap-- > 0) {
INTEGER(r)[nap] = INTEGER(rval)[nap];
SET_STRING_ELT(s, nap, STRING_ELT(nval, nap));
}
UNPROTECT(2);
return r;
}
UNPROTECT(1);
return rval;
}
// observe the raw infomation, but just like human ut will use some method to filter the noice
// this is just for one robot
void RobotTracker::observe(VisionRawInfo obs, double timestamp)
{
//如果有1秒没有来更新信号，自动复位，防止繁殖的程序内存不够
if(timestamp-time>0.5)
{
printf("Robot obvserve reset\r\n");
reset_on_observation = true;
}
// if there is no renew signal
// if the observation has been reset
if (reset_on_observation)
{
if (obs.conf <= 0.0)
{
return;
}
static Matrix observe_matrix(7,1), P(7);
observe_matrix.e(0,0) = obs.pos.x;
observe_matrix.e(1,0) = obs.pos.y;
observe_matrix.e(2,0) = obs.angle;
observe_matrix.e(3,0) = 0.0;
observe_matrix.e(4,0) = 0.0;
observe_matrix.e(5,0) = 0.0;
observe_matrix.e(6,0) = 0.0;
P.e(0,0) = DVAR(ROBOT_POSITION_VARIANCE);
P.e(1,1) = DVAR(ROBOT_POSITION_VARIANCE);
P.e(2,2) = DVAR(ROBOT_THETA_VARIANCE);
P.e(3,3) = 0.0; // 0m/s
if (type == ROBOT_TYPE_DIFF)
{
P.e(4,4) = 0.0;
}
else
{
P.e(4,4) = 0.0; // 0m/s
}
P.e(5,5) = 0.0;
P.e(6,6) = 0.0;
initial(obs.timestamp, observe_matrix, P);
reset_on_observation = false;
}
else
{
// If this is a new observation.
if (timestamp > time)
{
// Tick to current time.
tick(timestamp - time);
// Make observation
if (obs.timestamp == timestamp)
{
double xtheta = xs[0].e(2,0);
Matrix o(3,1);
o.e(0,0) = obs.pos.x;
o.e(1,0) = obs.pos.y;
o.e(2,0) = anglemod(obs.angle - xtheta) + xtheta;
update(o);
}
if (error_time_elapsed() > 10.0)
{
fprintf(stderr, "Kalman Error (pos, theta, vpos, vtheta): ");
fprintf(stderr, "%f ",
hypot(error_mean().e(0, 0), error_mean().e(1, 0)));
fprintf(stderr, "%f ", error_mean().e(2, 0));
fprintf(stderr, "%f ",
hypot(error_mean().e(3, 0), error_mean().e(4, 0)));
fprintf(stderr, "%f\n", error_mean().e(5, 0));
error_reset();
}
}
}
}
