void KalmanFilter::init(Eigen::Vector3f jerk_std,
Eigen::Vector3f measur_std,
float delta_t,
Eigen::Matrix<float, 9, 1> x_k1,
Eigen::Matrix<float, 9, 9> init_cov)
{
x_k_n_ = x_k1;
P_k_n_ = init_cov;
H_.fill(0.);
H_(0,0) = 1.;
H_(1,1) = 1.;
H_(2,2) = 1.;
R_.fill(0.);
R_(0,0) = pow(measur_std(0),2);
R_(1,1) = pow(measur_std(1),2);
R_(2,2) = pow(measur_std(2),2);
sigma_jerk_ = Eigen::Matrix3f::Zero();
sigma_jerk_(0,0) = pow(jerk_std(0),2);
sigma_jerk_(1,1) = pow(jerk_std(1),2);
sigma_jerk_(2,2) = pow(jerk_std(2),2);
if (delta_t <=0)
delta_t_ = 1/30; // We aim for 30 FPS
else
delta_t_ = delta_t;
delta_change();
}
inline
int compare_points(void *x, void *y)
{
point& a = *( (point*) x );
point& b = *( (point*) y );
if( H==NULL )
return cmp( D_(a.i,a.j), D_(b.i,b.j) );
else
return cmp( D_(a.i,a.j)+H_(a.i,a.j), D_(b.i,b.j)+H_(b.i,b.j) );
}
static void receive_avstream(conn* conn)
{
#define RTP_HEADER_SIZE 12
#define PACKET_SIZE 188
unsigned char payload[PACKET_SIZE + RTP_HEADER_SIZE] = {0};
for(;;) {
int nread = recvfrom(conn->sockfd, (void *)payload, sizeof(payload), 0, ( struct sockaddr *)&conn->serveraddr, (socklen_t *)&conn->client_addr_len);
if(nread < 0) { // timeout
ERR("no stream");
continue;
}
if(nread != PACKET_SIZE + RTP_HEADER_SIZE) {
ERR("rtp: %d / %d", nread, PACKET_SIZE + RTP_HEADER_SIZE);
H_(payload, 16);
}
else {
fprintf(stderr, "@\r");
#if (RTP_HEADER_SIZE == 12)
memmove(payload, &payload[12], 188);
#endif
KASSERT(payload[0] == 0x47);
unsigned short pid = ((uint16_t)(payload[1] & 0x1f) << 8) + payload[2];
if(pid == 0) { // PAT
dvbpsi_PushPacket(conn->hdvbpsi, payload);
}
}
// H_(payload, 16);
}
leave(conn);
}
// test the heap validity
void check_heap( int i, int j )
{
for( int x=0; x<n; ++x )
for( int y=0; y<p; ++y )
{
if( heap_pool_(x,y)!=NULL )
{
point& pt = * (point*)heap_pool_(x,y)->fhe_data;
if( H==NULL )
{
if( D_(i,j)>D_(pt.i,pt.j) )
ERROR_MSG("Problem with heap.\n");
}
else
{
if( D_(i,j)+H_(i,j)>D_(pt.i,pt.j)+H_(pt.i,pt.j) )
ERROR_MSG("Problem with heap.\n");
}
}
}
}
Vector Controller::computedxFP(const Matrix &H, const Vector &v,
const Vector &w, const Vector &x_FP)
{
Matrix H_=H;
Vector w_=w;
w_.push_back(1.0);
H_(0,3)=x_FP[0]-H_(0,3);
H_(1,3)=x_FP[1]-H_(1,3);
H_(2,3)=x_FP[2]-H_(2,3);
Vector dx_FP_pos=v+(w_[0]*cross(H_,0,H_,3)+
w_[1]*cross(H_,1,H_,3)+
w_[2]*cross(H_,2,H_,3));
H_(0,3)=H_(1,3)=H_(2,3)=0.0;
Vector dx_FP_rot=H_*w_;
dx_FP_rot.pop_back();
return cat(dx_FP_pos,dx_FP_rot);
}
void KalmanOwt::initialize(const double device_time, const double localTimeSecs) {
x_.setZero();
x_[0] = localTimeSecs - device_time;
P_.setZero();
P_(0,0) = config.sigmaInitOffset * config.sigmaInitOffset;
P_(1,1) = config.sigmaInitSkew * config.sigmaInitSkew;
applyConfig();
H_.setZero();
H_(0,0) = 1;
lastUpdateDeviceTime_ = device_time;
isInitialized_ = true;
}
void TimeSyncFilter::initialize(double device_time, double local_time) {
x_.setZero();
x_[0] = local_time - device_time;
P_.setZero();
P_(0, 0) = kSigmaInitOffset * kSigmaInitOffset;
P_(1, 1) = kSigmaInitSkew * kSigmaInitSkew;
Q_.setZero();
Q_(0, 0) = kSigmaOffset * kSigmaOffset;
Q_(1, 1) = kSigmaSkew * kSigmaSkew;
R_ = kSigmaMeasurementTimeOffset * kSigmaMeasurementTimeOffset;
H_.setZero();
H_(0, 0) = 1;
last_update_device_time_ = device_time;
is_initialized_ = true;
}
static void GrokNavigationMask(void) {
extern int navigation_mask;
navigation_mask = GMenuItemParseMask(H_("NavigationMask|None"));
}
