C++ (Cpp) state::rot_acc_lpf Examples

Example #1

File: ins.cpp Project: aeickho/pix32_uart_demo

/// update state using a rotation speed (in rad/s) and acceleration (in m/s^2)
void update(state & s, double timestep, vector3d const& rotspeed, vector3d const& acc)
{
	double thalf = 0.5*timestep;
	matrix3d const rhalf = rotation_vec_to_matrix(rotspeed*thalf);
	matrix3d const ohalf = s.orientation * rhalf;
	vector3d const racc  = ohalf * acc;
	vector3d const racchp = racc - s.rot_acc_lpf(racc);
	vector3d const shinc = thalf * racchp;
	vector3d const shalf = s.speed + shinc;
	s.location.noalias() += shalf * timestep;
	s.speed.noalias() = shalf + shinc;
	s.orientation.noalias() = ohalf * rhalf;
}