From e42655e7c66d40e7f60f7ba07b719cbe50aad50c Mon Sep 17 00:00:00 2001 From: tnaegeli Date: Mon, 1 Oct 2012 17:21:59 +0200 Subject: [PATCH] EKF --- .../attitude_estimator_ekf_main.c | 29 +++++++++++++++---- nuttx/configs/px4fmu/nsh/defconfig | 4 +-- 2 files changed, 25 insertions(+), 8 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 867b484e1d..e283fc59f4 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -271,7 +271,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) overloadcounter++; } - int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; + int8_t update_vect[3] = {1, 1, 1}; float euler[3]; int32_t z_k_sizes = 9; float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; @@ -282,11 +282,28 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) { dt = 0.005f; - knownConst[0] = 0.6f*0.6f*dt; - knownConst[1] = 0.6f*0.6f*dt; - knownConst[2] = 0.2f*0.2f*dt; - knownConst[3] = 0.2f*0.2f*dt; - knownConst[4] = 0.000001f*0.000001f*dt; // -9.81,1,1,-1}; + q[0] = 1e1; + q[1] = 1e1; + q[2] = 1e1; + q[3] = 1e-6; + q[4] = 1e-6; + q[5] = 1e-6; + q[6] = 1e-1; + q[7] = 1e-1; + q[8] = 1e-1; + q[9] = 1e-1; + q[10] = 1e-1; + q[11] = 1e-1; + + r[0]= 1e-2; + r[1]= 1e-2; + r[2]= 1e-2; + r[3]= 1e-1; + r[4]= 1e-1; + r[5]= 1e-1; + r[6]= 1e-1; + r[7]= 1e-1; + r[8]= 1e-1; x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index e1ba862cf7..f49fc61072 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -236,8 +236,8 @@ CONFIG_AT24XX_MTD_BLOCKSIZE=256 CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y -CONFIG_USART1_SERIAL_CONSOLE=y -CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART1_SERIAL_CONSOLE=n +CONFIG_USART2_SERIAL_CONSOLE=y CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n CONFIG_UART5_SERIAL_CONSOLE=n