void pos_init(void)
{
ASSERT_ONCE();
/* read configuration and initialize scl gates: */
opcd_param_t params[] =
{
{"process_noise", &process_noise},
{"ultra_noise", &ultra_noise},
{"baro_noise", &baro_noise},
{"gps_noise", &gps_noise},
{"use_gps_speed", &use_gps_speed},
OPCD_PARAMS_END
};
opcd_params_apply("kalman_pos.", params);
LOG(LL_DEBUG, "process noise: %f, ultra noise: %f, baro noise: %f, gps noise: %f",
tsfloat_get(&process_noise),
tsfloat_get(&ultra_noise),
tsfloat_get(&baro_noise),
tsfloat_get(&gps_noise));
/* set-up kalman filters: */
kalman_init(&e_kalman, &process_noise, &gps_noise, 0, 0, tsint_get(&use_gps_speed));
kalman_init(&n_kalman, &process_noise, &gps_noise, 0, 0, tsint_get(&use_gps_speed));
kalman_init(&baro_u_kalman, &process_noise, &baro_noise, 0, 0, false);
kalman_init(&ultra_u_kalman, &process_noise, &ultra_noise, 0, 0, false);
}
void init_ncurses_ui(double graph_limit_in, double hz_in, char use_colors_in)
{
graph_limit = graph_limit_in;
hz = hz_in;
use_colors = use_colors_in;
initscr();
start_color();
keypad(stdscr, TRUE);
intrflush(stdscr, FALSE);
noecho();
refresh();
nodelay(stdscr, FALSE);
meta(stdscr, TRUE); /* enable 8-bit input */
idlok(stdscr, TRUE); /* may give a little clunky screenredraw */
idcok(stdscr, TRUE); /* may give a little clunky screenredraw */
leaveok(stdscr, FALSE);
init_pair(C_WHITE, COLOR_WHITE, COLOR_BLACK);
init_pair(C_CYAN, COLOR_CYAN, COLOR_BLACK);
init_pair(C_MAGENTA, COLOR_MAGENTA, COLOR_BLACK);
init_pair(C_BLUE, COLOR_BLUE, COLOR_BLACK);
init_pair(C_YELLOW, COLOR_YELLOW, COLOR_BLACK);
init_pair(C_GREEN, COLOR_GREEN, COLOR_BLACK);
init_pair(C_RED, COLOR_RED, COLOR_BLACK);
kalman_init(0.0);
recreate_terminal();
}
//Main Function
int main(void)
{
double error_p = 0.0;
double error_i = 0.0;
double error_d = 0.0;
double angle;
kalman_state k = kalman_init(1, 16, 1, 137);
init_devices();
lcd_set_4bit();
lcd_init();
double preverror = 0.0;
while(1)
{
sensor2 = ADC_Conversion(2);
kalman_update(&k,(double)sensor2);
angle = k.x;
lcd_print(1,5,(int)angle,3);
error_p = (angle - 137);
error_i += error_p*dt;
error_d = (error_p - preverror)/dt;
lcd_print(1,1,abs((int)error_p),3);
Disturbance = ((error_p*kp) + (error_i*ki) + (error_d*kd));
if(angle > 137) back();
else { forward();}
if (Disturbance > MAX) Disturbance = MAX;
if (Disturbance < MIN) Disturbance = MIN;
lcd_print(2,6,abs((int)Disturbance),5);
velocity(35+abs((int)Disturbance),35+abs((int)Disturbance));
preverror = error_p;
}
}
static void
vd_init_timings(video_decoder_t *vd)
{
kalman_init(&vd->vd_avfilter);
vd->vd_prevpts = AV_NOPTS_VALUE;
vd->vd_nextpts = AV_NOPTS_VALUE;
vd->vd_estimated_duration = 0;
}
int main(void)
{
lb_init(&lb);
event_init();
motor_init();
uart_init(); // init USART
enc_init();
i2c_init();
adc_init();
kalman_init();
sei(); // enable interrupts
// Wait a second at startup
_delay_ms(1000);
// send initial string
printf_P(PSTR("Hello world!\n"));
imu_init();
for (;/*ever*/;)
{
// ADCSRA |= (1<<ADSC); // Set start conversion bit and wait for conversion to finish
// while(ADCSRA&(1<<ADSC));
// OCR1AL = ADCH; // Set ADC reading to timer 0 compare
if(event_pending())
{
event_action();
}
else // No pending operation, do low priority tasks
{
// dequeue receive buffer if any bytes waiting
while (uart_avail())
{
char c = uart_getc();
if (lb_append(&lb, c) == LB_BUFFER_FULL)
{
lb_init(&lb); // Clear line
printf_P(PSTR("\nMax line length exceeded\n"));
}
// Process command if line buffer is ready ...
if (lb_line_ready(&lb))
{
strcpy(cmd_string,lb_gets(&lb));
do_cmd(cmd_string);
lb_init(&lb);
}
}
}
// Process command if line buffer is terminated by a line feed or carriage return
}
return 0;
}
int main (void){
float acc_x, acc_z, gyro_x;
float acc_angle,kal_angle;
/* Init Systick to 1ms */
SysTick_Config( SystemCoreClock / 1000);
/* Initialize GPIO (sets up clock) */
LPC_SYSCON->SYSAHBCLKCTRL |= (1<<6);
SERVO_init();
if(I2CInit(I2CMASTER) == FALSE){
while(1); /* fatal error */
}
if(MPU6050_init()){
return 0;
}
MPU6050_setZero();
kalman_init();
for(;;){
/* 100Hz loop */
if(gSysTick_10 >= 9){
gSysTick_10 = 0;
/* get sensor values */
gyro_x = MPU6050_getGyroRoll_degree();
acc_x = MPU6050_getAccel_x();
acc_z = -MPU6050_getAccel_z();
/* acc angle */
acc_angle = atan2(acc_x , -acc_z) * 180/3.14159 ; // calculate accel angle
kal_angle = kalman_update(acc_angle,gyro_x, 0.01);
SERVO_set_slew((-kal_angle) - MECH_OFFSET);
}
}
}
void attitude_tobi_laurens_init(void)
{
//X Kalmanfilter
//initalize matrices
static m_elem kal_a[12 * 12] =
{ 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f };
static m_elem kal_c[9 * 12] =
{ 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f };
#define FACTOR 0.5f
#define FACTORstart 1.0f
static m_elem kal_gain[12 * 9] =
{ 0.004f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0.004f , 0 , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0 , 0.004f , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0 , 0 , 0.06f, 0 , 0 , 0 , 0 , 0,
0 , 0 , 0 , 0 , 0.06f, 0 , 0 , 0 , 0,
0 , 0 , 0 , 0 , 0 , 0.06f, 0 , 0 , 0,
0.0000f , +0.0000002f,0 , 0 , 0, 0, 0, 0 , 0,
-0.0000002f,0 , 0 , 0 , 0, 0, 0, 0, 0,
0, 0 , 0 , 0, 0, 0, 0, 0, 0,
0 , 0 , 0 , 0 , 0 , 0 , 0.9f , 0 , 0,
0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.9f , 0,
0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.9f
};
#define K 10.0f*TIME_STEP
static m_elem kal_gain_start[12 * 9] =
{ K, 0, 0, 0, 0, 0, 0, 0, 0,
0, K, 0, 0, 0, 0, 0, 0, 0,
0, 0, K, 0, 0, 0, 0, 0, 0,
0, 0, 0, K, 0, 0, 0, 0, 0,
0, 0, 0, 0, K, 0, 0, 0, 0,
0, 0, 0, 0, 0, K, 0, 0, 0,
0,0.00008f, 0, 0, 0, 0, 0, 0, 0,
-0.00008f ,0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, K,
0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0 };
static m_elem kal_x_apriori[12 * 1] =
{ };
//---> initial states sind aposteriori!? ---> fehler
static m_elem kal_x_aposteriori[12 * 1] =
{ 0, 0, -9.81f, 0.f, -0.2f, -0.9f, 0, 0, 0, 0, 0, 0 };
kalman_init(&attitude_tobi_laurens_kal, 12, 9, kal_a, kal_c,
kal_gain_start, kal_gain, kal_x_apriori, kal_x_aposteriori, 1000);
}
int main(void)
{
i2c_bus_t bus;
int ret = i2c_bus_open(&bus, "/dev/i2c-3");
if (ret < 0)
{
fatal("could not open i2c bus", ret);
return EXIT_FAILURE;
}
/* ITG: */
itg3200_dev_t itg;
itg_again:
ret = itg3200_init(&itg, &bus, ITG3200_DLPF_42HZ);
if (ret < 0)
{
fatal("could not inizialize ITG3200", ret);
if (ret == -EAGAIN)
{
goto itg_again;
}
return EXIT_FAILURE;
}
/* BMA: */
bma180_dev_t bma;
bma180_init(&bma, &bus, BMA180_RANGE_4G, BMA180_BW_10HZ);
/* HMC: */
hmc5883_dev_t hmc;
hmc5883_init(&hmc, &bus);
/* MS: */
ms5611_dev_t ms;
ret = ms5611_init(&ms, &bus, MS5611_OSR4096, MS5611_OSR4096);
if (ret < 0)
{
fatal("could not inizialize MS5611", ret);
return EXIT_FAILURE;
}
pthread_t thread;
pthread_create(&thread, NULL, ms5611_reader, &ms);
/* initialize AHRS filter: */
madgwick_ahrs_t madgwick_ahrs;
madgwick_ahrs_init(&madgwick_ahrs, STANDARD_BETA);
interval_t interval;
interval_init(&interval);
float init = START_BETA;
udp_socket_t *socket = udp_socket_create("10.0.0.100", 5005, 0, 0);
/* kalman filter: */
kalman_t kalman1, kalman2, kalman3;
kalman_init(&kalman1, 1.0e-6, 1.0e-2, 0, 0);
kalman_init(&kalman2, 1.0e-6, 1.0e-2, 0, 0);
kalman_init(&kalman3, 1.0e-6, 1.0e-2, 0, 0);
vec3_t global_acc; /* x = N, y = E, z = D */
int init_done = 0;
int converged = 0;
sliding_avg_t *avg[3];
avg[0] = sliding_avg_create(1000, 0.0);
avg[1] = sliding_avg_create(1000, 0.0);
avg[2] = sliding_avg_create(1000, -9.81);
float alt_rel_last = 0.0;
int udp_cnt = 0;
while (1)
{
int i;
float dt = interval_measure(&interval);
init -= BETA_STEP;
if (init < FINAL_BETA)
{
init = FINAL_BETA;
init_done = 1;
}
madgwick_ahrs.beta = init;
/* sensor data acquisition: */
itg3200_read_gyro(&itg);
bma180_read_acc(&bma);
hmc5883_read(&hmc);
/* state estimates and output: */
euler_t euler;
madgwick_ahrs_update(&madgwick_ahrs, itg.gyro.x, itg.gyro.y, itg.gyro.z, bma.raw.x, bma.raw.y, bma.raw.z, hmc.raw.x, hmc.raw.y, hmc.raw.z, 11.0, dt);
quat_t q_body_to_world;
quat_copy(&q_body_to_world, &madgwick_ahrs.quat);
quat_rot_vec(&global_acc, &bma.raw, &q_body_to_world);
for (i = 0; i < 3; i++)
{
global_acc.vec[i] -= sliding_avg_calc(avg[i], global_acc.vec[i]);
}
if (init_done)
{
kalman_in_t kalman_in;
kalman_in.dt = dt;
kalman_in.pos = 0;
kalman_out_t kalman_out;
kalman_in.acc = global_acc.x;
kalman_run(&kalman_out, &kalman1, &kalman_in);
kalman_in.acc = global_acc.y;
kalman_run(&kalman_out, &kalman2, &kalman_in);
kalman_in.acc = -global_acc.z;
pthread_mutex_lock(&mutex);
kalman_in.pos = alt_rel;
pthread_mutex_unlock(&mutex);
kalman_run(&kalman_out, &kalman3, &kalman_in);
if (!converged)
{
if (fabs(kalman_out.pos - alt_rel) < 0.1)
{
converged = 1;
fprintf(stderr, "init done\n");
}
}
if (converged) // && udp_cnt++ > 10)
{
if (udp_cnt++ == 10)
{
char buffer[1024];
udp_cnt = 0;
int len = sprintf(buffer, "%f %f %f %f %f %f %f", madgwick_ahrs.quat.q0, madgwick_ahrs.quat.q1, madgwick_ahrs.quat.q2, madgwick_ahrs.quat.q3,
global_acc.x, global_acc.y, global_acc.z);
udp_socket_send(socket, buffer, len);
}
printf("%f %f %f\n", -global_acc.z, alt_rel, kalman_out.pos);
fflush(stdout);
}
}
}
return 0;
}
void optflow_speed_kalman_init(void)
{
// kalman filter parameters
ax = global_data.param[PARAM_KAL_VEL_AX];
bx = global_data.param[PARAM_KAL_VEL_BX];
cx = 1.0;
ay = global_data.param[PARAM_KAL_VEL_AY];
by = global_data.param[PARAM_KAL_VEL_BY];
cy = 1.0;
// assumes initial state is 0
vx = 0.0;
vy = 0.0;
// assumes initial error covariance is 0
pvx = 0.0;
pvy = 0.0;
// noise parameters (hard-coded)
Qx = 0.005;
Rx = 0.2;
Qy = 0.005;
Ry = 0.2;
// set the optical flow scale (assuming linear relation)
// scale = -0.0714;
// scale = 0.04;
scale = 0.0008 / VEL_KF_TIME_STEP_X;
#ifdef IMU_PIXHAWK_V260_EXTERNAL_MAG
//Altitude Kalmanfilter
//initalize matrices
#define TIME_STEP_Z (1.0f / 50.0f)
static m_elem kal_z_a[4 * 4] =
{ 1.0f, TIME_STEP_Z, TIME_STEP_Z * TIME_STEP_Z / 2.0f, 0,
0, 1.0f, TIME_STEP_Z, 0 ,
0, 0, 1.0f, 0 ,
0, 0, 0, 1.0f };
static m_elem kal_z_c[2*4] =
{
1.0f, 0, 0, 0 ,
0, 0, 1.0f, 1.0f };
// static m_elem kal_z_gain[4 * 2] =
// {
// 0.0148553889079401, 3.73444963864759e-08,
// 0.00555539506146299, 1.49106022715582e-05,
// 0.000421844252811475, 0.997017766710577,
// -0.000421844052617397, 9.97097528182815e-08
// };
static m_elem kal_z_gain[4 * 2] =
{
0.0211952061386090, 0.00432160006966059,
0.0117007358555860, 0.00543786671309973,
0.00319895063370895, 0.00357092204456261,
-9.66582160118677e-07, 1.19075602845418e-06
};
// static m_elem kal_z_gain_start[4*2] =
// {
// 0.060188321659420, 3.566208652525075e-16 ,
// 0.008855645697701, 1.495920063190432e-13 ,
// 6.514669086807784e-04, 9.997000796699675e-08 ,
// -6.514669086807778e-04, 0.999700079925069 };
static m_elem kal_z_gain_start[4*2] =
{
1.0f+0*0.060188321659420, 3.566208652525075e-16 ,
0*0.008855645697701, 0*1.495920063190432e-13 ,
0*6.514669086807784e-04, 0*9.997000796699675e-08 ,
0*-6.514669086807778e-04, 0.999700079925069 };
static m_elem kal_z_x_apriori[4*1] =
{
-430 ,
0 ,
0 ,
-9.81 };
static m_elem kal_z_x_aposteriori[4*1] =
{
-430 ,
0 ,
0 ,
-9.81 };
kalman_init(&outdoor_position_kalman_z, 4, 2, kal_z_a, kal_z_c,
kal_z_gain_start, kal_z_gain, kal_z_x_apriori, kal_z_x_aposteriori,
100);
#endif
}
// Nullify measurement-values
void nullify_measurements() {
time_last_control_update = time_since_startup;
acc_zk = (fVector3){.t = time_last_control_update, .x=0.0, .y=0.0, .z=0.0};
acc_n = 0.0;
mag_zk = (fVector3){.t = time_last_control_update, .x=0.0, .y=0.0, .z=0.0};
mag_n = 0.0;
gyro_zk = (fVector3){.t = time_last_control_update, .x=0.0, .y=0.0, .z=0.0};
gyro_n = 0.0;
angle_target = (fVector3){.t = time_last_control_update, .x=0.0, .y=0.0, .z=0.0};
}
// Start controller
void Init_Controller() {
nullify_measurements();
angle_target = (fVector3){.t = 0, .x=0.0, .y=0.0, .z=0.0};
power_target = 0.0;
kalman_init(&kal_pitch);
kalman_init(&kal_roll);
#ifdef LOCAL_TEST_ENVIRONMENT
fp_out = fopen(OUT_FILE, "w");
if (!fp_out) {
printf("Could not open %s for writing\n", OUT_FILE);
}
#endif
}
void Close_Controller() {
#ifdef LOCAL_TEST_ENVIRONMENT
if (fp_out) {
fclose(fp_out);
}
#endif
}
inline void avg_measurement(fVector3* val, float n) {
val->x /= n;
val->y /= n;
val->z /= n;
}
inline int signum(int var) {
return (0 < var) - (var < 0);
}
// Currently this only optimizes for pitch_torque
//
// 7 kN thrust max at 338 us speed-setting with about 25cm arms
// --> 5 Nm/unit torque
// for yaw unknow, assuming 1 Nm/unit for now
//
// Order in which targets will be tried to be met:
// max_power -> pitch -> roll -> yaw
void calculate_motor_power(uint16_t *speeds, float32_t Lx, float32_t Ly, float32_t Lz) {
float32_t m1, m2, m3, m4;
float32_t F = (float32_t) power_target;
// Calc PITCH-axis
float32_t pitch_power = F / 2.0;
float32_t diff_pitch_power = Lx * PITCH_TORQUE_PER_UNIT;
if ( diff_pitch_power > pitch_power) {
m2 = 0.0;
m4 = pitch_power;
} else if ( ( - diff_pitch_power ) > pitch_power ) {
m2 = pitch_power;
m4 = 0.0;
} else {
m2 = pitch_power - diff_pitch_power;
m4 = pitch_power + diff_pitch_power;
}
// Calc ROLL-axis
float32_t roll_power = F / 2.0;
float32_t diff_roll_power = Ly * ROLL_TORQUE_PER_UNIT;
if ( diff_roll_power > roll_power) {
m1 = 0.0;
m3 = roll_power;
} else if ( ( - diff_roll_power ) > roll_power ) {
m1 = roll_power;
m3 = 0.0;
} else {
m1 = roll_power - diff_roll_power;
m3 = roll_power + diff_roll_power;
}
speeds[0] = round(m1);
speeds[1] = round(m2);
speeds[2] = round(m3);
speeds[3] = round(m4);
}
uint8_t update_motor_settings(uint16_t *speeds) {
float32_t dt, current_time, z1, z2;
float32_t pitch_err, pitch_torque, pitch_rate_target, pitch_rate_error;
float32_t roll_err, roll_torque, roll_rate_target, roll_rate_error;
// printf("%i / %i \n", time_since_startup, time_last_control_update);
if (!kalman_initialized && (acc_n > KALMAN_NUM_INIT_MEASUREMENTS)) {
avg_measurement(&acc_zk, acc_n);
avg_measurement(&mag_zk, mag_n);
avg_measurement(&gyro_zk, gyro_n);
kal_pitch.x1 = atan2(acc_zk.y, acc_zk.z);
kal_pitch.x2 = 0.0;
kal_pitch.x3 = (RADS_PER_DEGREE*gyro_zk.y);
kal_roll.x1 = atan2(acc_zk.x, acc_zk.z);
kal_roll.x2 = 0.0;
kal_roll.x3 = (RADS_PER_DEGREE*gyro_zk.x);
kalman_initialized = 1;
return 0;
} else if (!kalman_initialized) {
return 0;
} else if ((time_last_control_update + CONTROL_SAMPLE_RATE < time_since_startup)
&& (acc_n>0) && (gyro_n>0)) {
current_time = time_since_startup;
dt = ( (float32_t) (current_time - time_last_control_update)) / 1000.0;
time_last_control_update = current_time;
// Per meetserie een estimate maken van de huidige waarde door het gemiddelde te nemen
avg_measurement(&acc_zk, acc_n);
avg_measurement(&mag_zk, mag_n);
avg_measurement(&gyro_zk, gyro_n);
z1 = atan2(acc_zk.y, acc_zk.z);
z2 = gyro_zk.y*RADS_PER_DEGREE;
kalman_innovate(&kal_pitch, z1, z2, dt);
#ifndef LOCAL_TEST_ENVIRONMENT
LedAngle(kal_pitch.x1 * DEGREES_PER_RAD * 10);
#endif
pitch_err = kal_pitch.x1 - angle_target.x;
pitch_rate_target = do_pid(&PID_pitch, pitch_err, dt);
pitch_rate_error = kal_pitch.x2 - pitch_rate_target;
pitch_torque = do_pid(&PID_pitch_rate, pitch_rate_error, dt);
#ifdef LOCAL_TEST_ENVIRONMENT
if (fp_out)
{
fprintf(fp_out, "%d\t%.7f\t%.7f\t%.7f\t%.7f\t%.7f\n",
time_since_startup, kal_pitch.x1, kal_pitch.x2, kal_pitch.x3, z1, z2);
}
else {
printf("%d\t%.5f\t%.5f\t%.5f\t%.5f\t%.5f\n",
time_since_startup, kal_pitch.x1, kal_pitch.x2, kal_pitch.x3, z1, z2);
}
#endif
z1 = atan2(acc_zk.x, acc_zk.z);
z2 = gyro_zk.x*RADS_PER_DEGREE;
kalman_innovate(&kal_roll, z1, z2, dt);
roll_err = kal_roll.x1 - angle_target.x;
roll_rate_target = do_pid(&PID_roll, roll_err, dt);
roll_rate_error = kal_roll.x2 - roll_rate_target;
roll_torque = do_pid(&PID_roll_rate, roll_rate_error, dt);
calculate_motor_power(&(speeds[0]), pitch_torque, roll_torque, 0.0);
#ifdef LOCAL_TEST_ENVIRONMENT
printf("%04i %04i %04i %04i %f %f\n", speeds[0], speeds[1], speeds[2], speeds[3], pitch_torque, roll_torque);
#endif
nullify_measurements();
return 1;
}
return 0;
}
// Setup the kalman data struct
void kalman_init(kalman_data *data)
{
data->x1 = 0.0f;
data->x2 = 0.0f;
data->x3 = 0.0f;
// Init P to diagonal matrix with large values since
// the initial state is not known
data->p11 = 1000.0f;
data->p12 = 0.0f;
data->p13 = 0.0f;
data->p21 = 0.0f;
data->p22 = 1000.0f;
data->p23 = 0.0f;
data->p31 = 0.0f;
data->p32 = 0.0f;
data->p33 = 1000.0f;
data->q1 = Q1;
data->q2 = Q2;
data->q3 = Q3;
data->r1 = R1;
data->r2 = R2;
}
void vision_position_kalman_init(void)
{
//X Kalmanfilter
//initalize matrices
#define TIME_STEP_X (1.0f / 200.0f)
static m_elem kal_x_a[4 * 4] =
{ 1.0f, TIME_STEP_X, TIME_STEP_X * TIME_STEP_X / 2.0f, 0,
0, VELOCITY_HOLD, TIME_STEP_X, 0 ,
0, 0, ACCELERATION_HOLD, 0 ,
0, 0, 0, 1.0f };
static m_elem kal_x_c[2*4] =
{
1.0f, 0, 0, 0 ,
0, 0, 1.0f, 1.0f };
static m_elem kal_x_gain[4 * 2] =
{
0.120089421679731, 4.35722593937886e-06,
0.213207108111745, 0.00113300623965082,
0.177947308239338, 0.772331751820938,
-0.177890740554481, 0.000772735355118873};
static m_elem kal_x_gain_start[4*2] =
{
0.184954631641736, 3.83974662133946e-14,
0.0273267826361096, 1.76786246429369e-11,
0.00201871900830449, 9.64639763760847e-08,
-0.00201871900823153, 0.964639764723918};
static m_elem kal_x_x_apriori[4*1] =
{
0 ,
0 ,
0 ,
0 };
static m_elem kal_x_x_aposteriori[4*1] =
{
0 ,
0 ,
0 ,
0 };
kalman_init(&vision_position_kalman_x, 4, 2, kal_x_a, kal_x_c,
kal_x_gain_start, kal_x_gain, kal_x_x_apriori, kal_x_x_aposteriori,
1000);
//Y Kalmanfilter
//initalize matrices
#define TIME_STEP_Y (1.0f / 200.0f)
static m_elem kal_y_a[4 * 4] =
{ 1.0f, TIME_STEP_Y, TIME_STEP_Y * TIME_STEP_Y / 2.0f, 0,
0, VELOCITY_HOLD, TIME_STEP_Y, 0 ,
0, 0, ACCELERATION_HOLD, 0 ,
0, 0, 0, 1.0f };
static m_elem kal_y_c[2*4] =
{
1.0f, 0, 0, 0 ,
0, 0, 1.0f, 1.0f };
static m_elem kal_y_gain[4 * 2] =
{
0.120089421679731, 4.35722593937886e-06,
0.213207108111745, 0.00113300623965082,
0.177947308239338, 0.772331751820938,
-0.177890740554481, 0.000772735355118873};
static m_elem kal_y_gain_start[4*2] =
{
0.184954631641736, 3.83974662133946e-14,
0.0273267826361096, 1.76786246429369e-11,
0.00201871900830449, 9.64639763760847e-08,
-0.00201871900823153, 0.964639764723918};
static m_elem kal_y_x_apriori[4*1] =
{
0 ,
0 ,
0 ,
0 };
static m_elem kal_y_x_aposteriori[4*1] =
{
0 ,
0 ,
0 ,
0 };
kalman_init(&vision_position_kalman_y, 4, 2, kal_y_a, kal_y_c,
kal_y_gain_start, kal_y_gain, kal_y_x_apriori, kal_y_x_aposteriori,
1000);
//Altitude Kalmanfilter
//initalize matrices
#define TIME_STEP_Z (1.0f / 200.0f)
static m_elem kal_z_a[4 * 4] =
{ 1.0f, TIME_STEP_Z, TIME_STEP_Z * TIME_STEP_Z / 2.0f, 0,
0, VELOCITY_HOLD, TIME_STEP_Z, 0 ,
0, 0, ACCELERATION_HOLD, 0 ,
0, 0, 0, 1.0f };
static m_elem kal_z_c[2*4] =
{
1.0f, 0, 0, 0 ,
0, 0, 1.0f, 1.0f };
// static m_elem kal_z_gain[4 * 2] =
// {
// 0.120089421679731, 4.35722593937886e-06,
// 0.213207108111745, 0.00113300623965082,
// 0.177947308239338, 0.772331751820938,
// -0.177890740554481, 0.000772735355118873};
static m_elem kal_z_gain[4 * 2] =
{
0.120089421679731, 3.73444963864759e-08,
0.213207108111745, 1.49106022715582e-05,
0.177947308239338, 0.997017766710577,
-0.177890740554481, 9.97097528182815e-08};
static m_elem kal_z_gain_start[4*2] =
{
0.184954631641736, 3.83974662133946e-14,
0.0273267826361096, 1.76786246429369e-11,
0.00201871900830449, 9.64639763760847e-08,
-0.00201871900823153, 0.964639764723918};
static m_elem kal_z_x_apriori[4*1] =
{
0 ,
0 ,
0 ,
-9.81 };
static m_elem kal_z_x_aposteriori[4*1] =
{
0 ,
0 ,
0 ,
-9.81 };
kalman_init(&vision_position_kalman_z, 4, 2, kal_z_a, kal_z_c,
kal_z_gain_start, kal_z_gain, kal_z_x_apriori, kal_z_x_aposteriori,
1000);
}
void vicon_position_kalman_init(void)
{
//X Kalmanfilter
//initalize matrices
#define TIME_STEP_X (1.0f / 200.0f)
static m_elem kal_x_a[2 * 2] =
{ 1.0f, TIME_STEP_X,
0.0f, VELOCITY_HOLD
};
static m_elem kal_x_c[2 * 2] =
{ 1.0f, 0.0f,
1.0f, 0.0f };
static m_elem kal_x_gain[2 * 2] =
{
0.550311626986869, 0.809201491990525,
4.24117140900075, 5.04378836470466
};
static m_elem kal_x_gain_start[2 * 2] =
{
0.177673118212026, 0.363726574226735,
0, 0
};
static m_elem kal_x_x_apriori[2 * 1] =
{
0 ,
0 };
static m_elem kal_x_x_aposteriori[2 * 1] =
{
0 ,
0 };
kalman_init(&vicon_position_kalman_x, 2, 2, kal_x_a, kal_x_c,
kal_x_gain_start, kal_x_gain, kal_x_x_apriori, kal_x_x_aposteriori,
1000);
//Y Kalmanfilter
//initalize matrices
#define TIME_STEP_Y (1.0f / 200.0f)
static m_elem kal_y_a[2 * 2] =
{ 1.0f, TIME_STEP_Y,
0.0f, VELOCITY_HOLD
};
static m_elem kal_y_c[2 * 2] =
{ 1.0f, 0.0f,
1.0f, 0.0f };
static m_elem kal_y_gain[2 * 2] =
{
0.550311626986869, 0.809201491990525,
4.24117140900075, 5.04378836470466
};
static m_elem kal_y_gain_start[2 * 2] =
{
0.177673118212026, 0.363726574226735,
0, 0
};
static m_elem kal_y_x_apriori[2 * 1] =
{
0 ,
0 };
static m_elem kal_y_x_aposteriori[2 * 1] =
{
0 ,
0 };
kalman_init(&vicon_position_kalman_y, 2, 2, kal_y_a, kal_y_c,
kal_y_gain_start, kal_y_gain, kal_y_x_apriori, kal_y_x_aposteriori,
1000);
//Altitude Kalmanfilter
//initalize matrices
#define TIME_STEP_Z (1.0f / 200.0f)
static m_elem kal_z_a[2 * 2] =
{ 1.0f, TIME_STEP_Z,
0.0f, VELOCITY_HOLD
};
static m_elem kal_z_c[2 * 2] =
{ 1.0f, 0.0f,
1.0f, 0.0f };
// static m_elem kal_z_gain[2 * 2] =
// {
// 0.120089421679731, 4.35722593937886e-06,
// 0.213207108111745, 0.00113300623965082,
// 0.177947308239338, 0.772331751820938,
// -0.177890740554481, 0.000772735355118873};
static m_elem kal_z_gain[2 * 2] =
{
0.550311626986869, 0.809201491990525,
4.24117140900075, 5.04378836470466
};
// {
// 0.177673118212026, 0.363726574226735,
// 1.13547678406557, 1.65089930506536
// };
static m_elem kal_z_gain_start[2 * 2] =
{
0.177673118212026, 0.363726574226735,
0, 0
};
static m_elem kal_z_x_apriori[2 * 1] =
{
0 ,
0 };
static m_elem kal_z_x_aposteriori[2 * 1] =
{
0 ,
0 };
kalman_init(&vicon_position_kalman_z, 2, 2, kal_z_a, kal_z_c,
kal_z_gain_start, kal_z_gain, kal_z_x_apriori, kal_z_x_aposteriori,
1000);
}
/**
Time domain physical simulation.
noisy:
- 0: no noise at all;
- 1: poisson and read out noise.
- 2: only poisson noise.
*/
dmat *skysim_sim(dmat **mresout, const dmat *mideal, const dmat *mideal_oa, double ngsol,
ASTER_S *aster, const POWFS_S *powfs,
const PARMS_S *parms, int idtratc, int noisy, int phystart){
int dtratc=0;
if(!parms->skyc.multirate){
dtratc=parms->skyc.dtrats->p[idtratc];
}
int hasphy;
if(phystart>-1 && phystart<aster->nstep){
hasphy=1;
}else{
hasphy=0;
}
const int nmod=mideal->nx;
dmat *res=dnew(6,1);/*Results. 1-2: NGS and TT modes.,
3-4:On axis NGS and TT modes,
4-6: On axis NGS and TT wihtout considering un-orthogonality.*/
dmat *mreal=NULL;/*modal correction at this step. */
dmat *merr=dnew(nmod,1);/*modal error */
dcell *merrm=dcellnew(1,1);dcell *pmerrm=NULL;
const int nstep=aster->nstep?aster->nstep:parms->maos.nstep;
dmat *mres=dnew(nmod,nstep);
dmat* rnefs=parms->skyc.rnefs;
dcell *zgradc=dcellnew3(aster->nwfs, 1, aster->ngs, 0);
dcell *gradout=dcellnew3(aster->nwfs, 1, aster->ngs, 0);
dmat *gradsave=0;
if(parms->skyc.dbg){
gradsave=dnew(aster->tsa*2,nstep);
}
SERVO_T *st2t=0;
kalman_t *kalman=0;
dcell *mpsol=0;
dmat *pgm=0;
dmat *dtrats=0;
int multirate=parms->skyc.multirate;
if(multirate){
kalman=aster->kalman[0];
dtrats=aster->dtrats;
}else{
if(parms->skyc.servo>0){
const double dtngs=parms->maos.dt*dtratc;
st2t=servo_new(merrm, NULL, 0, dtngs, aster->gain->p[idtratc]);
pgm=aster->pgm->p[idtratc];
}else{
kalman=aster->kalman[idtratc];
}
}
if(kalman){
kalman_init(kalman);
mpsol=dcellnew(aster->nwfs, 1); //for psol grad.
}
const long nwvl=parms->maos.nwvl;
dcell **psf=0, **mtche=0, **ints=0;
ccell *wvf=0,*wvfc=0, *otf=0;
if(hasphy){
psf=mycalloc(aster->nwfs,dcell*);
wvf=ccellnew(aster->nwfs,1);
wvfc=ccellnew(aster->nwfs,1);
mtche=mycalloc(aster->nwfs,dcell*);
ints=mycalloc(aster->nwfs,dcell*);
otf=ccellnew(aster->nwfs,1);
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long ncomp=parms->maos.ncomp[ipowfs];
const long nsa=parms->maos.nsa[ipowfs];
wvf->p[iwfs]=cnew(ncomp,ncomp);
wvfc->p[iwfs]=NULL;
psf[iwfs]=dcellnew(nsa,nwvl);
//cfft2plan(wvf->p[iwfs], -1);
if(parms->skyc.multirate){
mtche[iwfs]=aster->wfs[iwfs].pistat->mtche[(int)aster->idtrats->p[iwfs]];
}else{
mtche[iwfs]=aster->wfs[iwfs].pistat->mtche[idtratc];
}
otf->p[iwfs]=cnew(ncomp,ncomp);
//cfft2plan(otf->p[iwfs],-1);
//cfft2plan(otf->p[iwfs],1);
ints[iwfs]=dcellnew(nsa,1);
int pixpsa=parms->skyc.pixpsa[ipowfs];
for(long isa=0; isa<nsa; isa++){
ints[iwfs]->p[isa]=dnew(pixpsa,pixpsa);
}
}
}
for(int irep=0; irep<parms->skyc.navg; irep++){
if(kalman){
kalman_init(kalman);
}else{
servo_reset(st2t);
}
dcellzero(zgradc);
dcellzero(gradout);
if(ints){
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
dcellzero(ints[iwfs]);
}
}
for(int istep=0; istep<nstep; istep++){
memcpy(merr->p, PCOL(mideal,istep), nmod*sizeof(double));
dadd(&merr, 1, mreal, -1);/*form NGS mode error; */
memcpy(PCOL(mres,istep),merr->p,sizeof(double)*nmod);
if(mpsol){//collect averaged modes for PSOL.
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
dadd(&mpsol->p[iwfs], 1, mreal, 1);
}
}
pmerrm=0;
if(istep>=parms->skyc.evlstart){/*performance evaluation*/
double res_ngs=dwdot(merr->p,parms->maos.mcc,merr->p);
if(res_ngs>ngsol*100){
dfree(res); res=NULL;
break;
}
{
res->p[0]+=res_ngs;
res->p[1]+=dwdot2(merr->p,parms->maos.mcc_tt,merr->p);
double dot_oa=dwdot(merr->p, parms->maos.mcc_oa, merr->p);
double dot_res_ideal=dwdot(merr->p, parms->maos.mcc_oa, PCOL(mideal,istep));
double dot_res_oa=0;
for(int imod=0; imod<nmod; imod++){
dot_res_oa+=merr->p[imod]*IND(mideal_oa,imod,istep);
}
res->p[2]+=dot_oa-2*dot_res_ideal+2*dot_res_oa;
res->p[4]+=dot_oa;
}
{
double dot_oa_tt=dwdot2(merr->p, parms->maos.mcc_oa_tt, merr->p);
/*Notice that mcc_oa_tt2 is 2x5 marix. */
double dot_res_ideal_tt=dwdot(merr->p, parms->maos.mcc_oa_tt2, PCOL(mideal,istep));
double dot_res_oa_tt=0;
for(int imod=0; imod<2; imod++){
dot_res_oa_tt+=merr->p[imod]*IND(mideal_oa,imod,istep);
}
res->p[3]+=dot_oa_tt-2*dot_res_ideal_tt+2*dot_res_oa_tt;
res->p[5]+=dot_oa_tt;
}
}//if evl
if(istep<phystart || phystart<0){
/*Ztilt, noise free simulation for acquisition. */
dmm(&zgradc->m, 1, aster->gm, merr, "nn", 1);/*grad due to residual NGS mode. */
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long ng=parms->maos.nsa[ipowfs]*2;
for(long ig=0; ig<ng; ig++){
zgradc->p[iwfs]->p[ig]+=aster->wfs[iwfs].ztiltout->p[istep*ng+ig];
}
}
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
int dtrati=(multirate?(int)dtrats->p[iwfs]:dtratc);
if((istep+1) % dtrati==0){
dadd(&gradout->p[iwfs], 0, zgradc->p[iwfs], 1./dtrati);
dzero(zgradc->p[iwfs]);
if(noisy){
int idtrati=(multirate?(int)aster->idtrats->p[iwfs]:idtratc);
dmat *nea=aster->wfs[iwfs].pistat->sanea->p[idtrati];
for(int i=0; i<nea->nx; i++){
gradout->p[iwfs]->p[i]+=nea->p[i]*randn(&aster->rand);
}
}
pmerrm=merrm;//record output.
}
}
}else{
/*Accumulate PSF intensities*/
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
const double thetax=aster->wfs[iwfs].thetax;
const double thetay=aster->wfs[iwfs].thetay;
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long nsa=parms->maos.nsa[ipowfs];
ccell* wvfout=aster->wfs[iwfs].wvfout[istep];
for(long iwvl=0; iwvl<nwvl; iwvl++){
double wvl=parms->maos.wvl[iwvl];
for(long isa=0; isa<nsa; isa++){
ccp(&wvfc->p[iwfs], IND(wvfout,isa,iwvl));
/*Apply NGS mode error to PSF. */
ngsmod2wvf(wvfc->p[iwfs], wvl, merr, powfs+ipowfs, isa,
thetax, thetay, parms);
cembedc(wvf->p[iwfs],wvfc->p[iwfs],0,C_FULL);
cfft2(wvf->p[iwfs],-1);
/*peak in corner. */
cabs22d(&psf[iwfs]->p[isa+nsa*iwvl], 1., wvf->p[iwfs], 1.);
}/*isa */
}/*iwvl */
}/*iwfs */
/*Form detector image from accumulated PSFs*/
double igrad[2];
for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
int dtrati=dtratc, idtrat=idtratc;
if(multirate){//multirate
idtrat=aster->idtrats->p[iwfs];
dtrati=dtrats->p[iwfs];
}
if((istep+1) % dtrati == 0){/*has output */
dcellzero(ints[iwfs]);
const int ipowfs=aster->wfs[iwfs].ipowfs;
const long nsa=parms->maos.nsa[ipowfs];
for(long isa=0; isa<nsa; isa++){
for(long iwvl=0; iwvl<nwvl; iwvl++){
double siglev=aster->wfs[iwfs].siglev->p[iwvl];
ccpd(&otf->p[iwfs],psf[iwfs]->p[isa+nsa*iwvl]);
cfft2i(otf->p[iwfs], 1); /*turn to OTF, peak in corner */
ccwm(otf->p[iwfs], powfs[ipowfs].dtf[iwvl].nominal);
cfft2(otf->p[iwfs], -1);
dspmulcreal(ints[iwfs]->p[isa]->p, powfs[ipowfs].dtf[iwvl].si,
otf->p[iwfs]->p, siglev);
}
/*Add noise and apply matched filter. */
#if _OPENMP >= 200805
#pragma omp critical
#endif
switch(noisy){
case 0:/*no noise at all. */
break;
case 1:/*both poisson and read out noise. */
{
double bkgrnd=aster->wfs[iwfs].bkgrnd*dtrati;
addnoise(ints[iwfs]->p[isa], &aster->rand, bkgrnd, bkgrnd, 0,0,IND(rnefs,idtrat,ipowfs));
}
break;
case 2:/*there is still poisson noise. */
addnoise(ints[iwfs]->p[isa], &aster->rand, 0, 0, 0,0,0);
break;
default:
error("Invalid noisy\n");
}
igrad[0]=0;
igrad[1]=0;
double pixtheta=parms->skyc.pixtheta[ipowfs];
if(parms->skyc.mtch){
dmulvec(igrad, mtche[iwfs]->p[isa], ints[iwfs]->p[isa]->p, 1);
}
if(!parms->skyc.mtch || fabs(igrad[0])>pixtheta || fabs(igrad[1])>pixtheta){
if(!parms->skyc.mtch){
warning2("fall back to cog\n");
}else{
warning_once("mtch is out of range\n");
}
dcog(igrad, ints[iwfs]->p[isa], 0, 0, 0, 3*IND(rnefs,idtrat,ipowfs), 0);
igrad[0]*=pixtheta;
igrad[1]*=pixtheta;
}
gradout->p[iwfs]->p[isa]=igrad[0];
gradout->p[iwfs]->p[isa+nsa]=igrad[1];
}/*isa */
pmerrm=merrm;
dcellzero(psf[iwfs]);/*reset accumulation.*/
}/*if iwfs has output*/
}/*for wfs*/
}/*if phystart */
//output to mreal after using it to ensure two cycle delay.
if(st2t){//Type I or II control.
if(st2t->mint->p[0]){//has output.
dcp(&mreal, st2t->mint->p[0]->p[0]);
}
}else{//LQG control
kalman_output(kalman, &mreal, 0, 1);
}
if(kalman){//LQG control
int indk=0;
//Form PSOL grads and obtain index to LQG M
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
int dtrati=(multirate?(int)dtrats->p[iwfs]:dtratc);
if((istep+1) % dtrati==0){
indk|=1<<iwfs;
dmm(&gradout->p[iwfs], 1, aster->g->p[iwfs], mpsol->p[iwfs], "nn", 1./dtrati);
dzero(mpsol->p[iwfs]);
}
}
if(indk){
kalman_update(kalman, gradout->m, indk-1);
}
}else if(st2t){
if(pmerrm){
dmm(&merrm->p[0], 0, pgm, gradout->m, "nn", 1);
}
servo_filter(st2t, pmerrm);//do even if merrm is zero. to simulate additional latency
}
if(parms->skyc.dbg){
memcpy(PCOL(gradsave, istep), gradout->m->p, sizeof(double)*gradsave->nx);
}
}/*istep; */
}
if(parms->skyc.dbg){
int dtrati=(multirate?(int)dtrats->p[0]:dtratc);
writebin(gradsave,"%s/skysim_grads_aster%d_dtrat%d",dirsetup, aster->iaster,dtrati);
writebin(mres,"%s/skysim_sim_mres_aster%d_dtrat%d",dirsetup,aster->iaster,dtrati);
}
dfree(mreal);
dcellfree(mpsol);
dfree(merr);
dcellfree(merrm);
dcellfree(zgradc);
dcellfree(gradout);
dfree(gradsave);
if(hasphy){
dcellfreearr(psf, aster->nwfs);
dcellfreearr(ints, aster->nwfs);
ccellfree(wvf);
ccellfree(wvfc);
ccellfree(otf);
free(mtche);
}
servo_free(st2t);
/*dfree(mres); */
if(mresout) {
*mresout=mres;
}else{
dfree(mres);
}
dscale(res, 1./((nstep-parms->skyc.evlstart)*parms->skyc.navg));
return res;
}
void outdoor_position_kalman_init(void)
{
//X Kalmanfilter
//initalize matrices
#define TIME_STEP_X (1.0f / 200.0f)
static m_elem kal_x_a[4 * 4] =
{ 1.0f, TIME_STEP_X, TIME_STEP_X * TIME_STEP_X / 2.0f, 0,
0, 1.0f, TIME_STEP_X, 0 ,
0, 0, 0.9f, 0 ,
0, 0, 0, 1.0f };
static m_elem kal_x_c[2*4] =
{
1.0f, 0, 0, 0 ,
0, 0, 1.0f, 1.0f };
//
// static m_elem kal_x_gain[4 * 2] =
// {
// 0.0148553889079401, 3.73444963864759e-08,
// 0.00555539506146299, 1.49106022715582e-05,
// 0.000421844252811475, 0.997017766710577,
// -0.000421844052617397, 9.97097528182815e-08 };
// static m_elem kal_x_gain[4 * 2] =
// {
// 0.374797938488850, 3.73850295872383e-08,
// 0.0853099645050014, 1.49106314199340e-05,
// 0.00111821605423848, 0.997017766714797,
// -0.00111821464754384, 9.97055320882196e-08};
// static m_elem kal_x_gain[4 * 2] =
// {
// 0.652373111233803, 3.69547422415130e-08,
// 0.335139052708543, 1.48803888106151e-05,
// 0.0833402202608038, 0.996024802404946,
// -0.0833402194802173, 0.000996025175585497};
// static m_elem kal_x_gain[4 * 2] =
// {
// 0.592750199387368, 4.43580350345266e-06,
// 0.261234180593370, 0.00113328056903801,
// 0.0520844935682290, 0.772332121775609,
// -0.0520796109459413, 0.000772365439149219};
static m_elem kal_x_gain[4 * 2] =
{
0.770934704461122, 4.40141801112333e-06,
0.536732919373398, 0.00113318742260124,
0.151314528891492, 0.772332026761219,
-0.151273373292308, 0.0772460449859003};//0.000772460449859003
// static m_elem kal_x_gain_start[4*2] =
// {
// 0.060188321659420, 3.566208652525075e-16 ,
// 0.008855645697701, 1.495920063190432e-13 ,
// 6.514669086807784e-04, 9.997000796699675e-08 ,
// -6.514669086807778e-04, 0.999700079925069 };
static m_elem kal_x_gain_start[4*2] =
{
0.0291725594968339, 4.20818895401493e-16,
0.00426373951034103, 1.50288378749457e-13,
0.000311581023720996, 9.99700080297815e-08,
-0.000311581023720996, 0.999700079925069};
static m_elem kal_x_x_apriori[4*1] =
{
0 ,
0 ,
0 ,
0 };
static m_elem kal_x_x_aposteriori[4*1] =
{
0 ,
0 ,
0 ,
0 };
kalman_init(&outdoor_position_kalman_x, 4, 2, kal_x_a, kal_x_c,
kal_x_gain_start, kal_x_gain, kal_x_x_apriori, kal_x_x_aposteriori,
1000);
//Y Kalmanfilter
//initalize matrices
#define TIME_STEP_Y (1.0f / 200.0f)
static m_elem kal_y_a[4 * 4] =
{ 1.0f, TIME_STEP_Y, TIME_STEP_Y * TIME_STEP_Y / 2.0f, 0,
0, 1.0f, TIME_STEP_Y, 0 ,
0, 0, 0.9f, 0 ,
0, 0, 0, 1.0f };
static m_elem kal_y_c[2*4] =
{
1.0f, 0, 0, 0 ,
0, 0, 1.0f, 1.0f };
//
// static m_elem kal_y_gain[4 * 2] =
// {
// 0.0148553889079401, 3.73444963864759e-08,
// 0.00555539506146299, 1.49106022715582e-05,
// 0.000421844252811475, 0.997017766710577,
// -0.000421844052617397, 9.97097528182815e-08 };
// static m_elem kal_y_gain[4 * 2] =
// {
// 0.374797938488850, 3.73850295872383e-08,
// 0.0853099645050014, 1.49106314199340e-05,
// 0.00111821605423848, 0.997017766714797,
// -0.00111821464754384, 9.97055320882196e-08};
// static m_elem kal_y_gain[4 * 2] =
// {
// 0.652373111233803, 3.69547422415130e-08,
// 0.335139052708543, 1.48803888106151e-05,
// 0.0833402202608038, 0.996024802404946,
// -0.0833402194802173, 0.000996025175585497};
// static m_elem kal_y_gain[4 * 2] =
// {
// 0.592750199387368, 4.43580350345266e-06,
// 0.261234180593370, 0.00113328056903801,
// 0.0520844935682290, 0.772332121775609,
// -0.0520796109459413, 0.000772365439149219};
static m_elem kal_y_gain[4 * 2] =
{
0.770934704461122, 4.40141801112333e-06,
0.536732919373398, 0.00113318742260124,
0.151314528891492, 0.772332026761219,
-0.151273373292308, 0.0772460449859003};//0.000772460449859003
// static m_elem kal_y_gain_start[4*2] =
// {
// 0.060188321659420, 3.566208652525075e-16 ,
// 0.008855645697701, 1.495920063190432e-13 ,
// 6.514669086807784e-04, 9.997000796699675e-08 ,
// -6.514669086807778e-04, 0.999700079925069 };
static m_elem kal_y_gain_start[4*2] =
{
0.0291725594968339, 4.20818895401493e-16,
0.00426373951034103, 1.50288378749457e-13,
0.000311581023720996, 9.99700080297815e-08,
-0.000311581023720996, 0.999700079925069};
static m_elem kal_y_x_apriori[4*1] =
{
0 ,
0 ,
0 ,
0 };
static m_elem kal_y_x_aposteriori[4*1] =
{
0 ,
0 ,
0 ,
0 };
kalman_init(&outdoor_position_kalman_y, 4, 2, kal_y_a, kal_y_c,
kal_y_gain_start, kal_y_gain, kal_y_x_apriori, kal_y_x_aposteriori,
1000);
//Altitude Kalmanfilter
//initalize matrices
#define TIME_STEP_Z (1.0f / 200.0f)
static m_elem kal_z_a[4 * 4] =
{ 1.0f, TIME_STEP_Z, TIME_STEP_Z * TIME_STEP_Z / 2.0f, 0,
0, 1.0f, TIME_STEP_Z, 0 ,
0, 0, 1.0f, 0 ,
0, 0, 0, 1.0f };
static m_elem kal_z_c[2*4] =
{
1.0f, 0, 0, 0 ,
0, 0, 1.0f, 1.0f };
static m_elem kal_z_gain[4 * 2] =
{
0.0148553889079401, 3.73444963864759e-08,
0.00555539506146299, 1.49106022715582e-05,
0.000421844252811475, 0.997017766710577,
-0.000421844052617397, 9.97097528182815e-08 };
static m_elem kal_z_gain_start[4*2] =
{
0.060188321659420, 3.566208652525075e-16 ,
0.008855645697701, 1.495920063190432e-13 ,
6.514669086807784e-04, 9.997000796699675e-08 ,
-6.514669086807778e-04, 0.999700079925069 };
static m_elem kal_z_x_apriori[4*1] =
{
0 ,
0 ,
0 ,
-9.81 };
static m_elem kal_z_x_aposteriori[4*1] =
{
0 ,
0 ,
0 ,
-9.81 };
kalman_init(&outdoor_position_kalman_z, 4, 2, kal_z_a, kal_z_c,
kal_z_gain_start, kal_z_gain, kal_z_x_apriori, kal_z_x_aposteriori,
1000);
}
/*
* allocates and initializes a kalman filter
*/
kalman_t *kalman_create(const kalman_config_t *config, const kalman_out_t *init_state)
{
kalman_t *kalman = (kalman_t *)malloc(sizeof(kalman_t));
kalman_init(kalman, config->process_var, config->measurement_var, init_state->pos, init_state->speed);
return kalman;
}
void openwsn_init() {
// drivers
openserial_init();
// stack
// cross-layer
idmanager_init(); // call first since initializes e.g. EUI64
openqueue_init();
openrandom_init();
opentimers_init();
// 02a-TSCH
ieee154e_init();
// 02b-RES
schedule_init();
res_init();
neighbors_init();
kalman_init();
adaptiveKalman_init();
nf_init(); //noise floor module
// 03a-IPHC
openbridge_init();
iphc_init();
// 03b-IPv6
forwarding_init();
icmpv6_init();
icmpv6echo_init();
icmpv6router_init();
icmpv6rpl_init();
// 04-TRAN
opentcp_init();
openudp_init();
opencoap_init(); // initialize before any of the CoAP clients
// 07-App
//--CoAP
//rwellknown_init();
//rreg_init();
//rinfo_init();
//rleds_init();
//rt_init();
//rex_init();
//rheli_init();
//rrube_init();
//rxl1_init();
//layerdebug_init();
//--UDP
/*udpecho_init();
udpinject_init();
udpprint_init();*/
//udprand_init();
//udpstorm_init();
//--TCP
/*ohlone_init();
tcpecho_init();
tcpinject_init();
tcpprint_init();*/
//--misc
//heli_init();
//imu_init();
//bbk_init();
//hdl_init();
}