diff --git a/src/examples/attitude_estimator_ekf/AttitudeEKF.m b/src/examples/attitude_estimator_ekf/AttitudeEKF.m deleted file mode 100644 index cf559919ae..0000000000 --- a/src/examples/attitude_estimator_ekf/AttitudeEKF.m +++ /dev/null @@ -1,298 +0,0 @@ -function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... - = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) - - -%LQG Position Estimator and Controller -% Observer: -% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) -% x[n+1|n] = Ax[n|n] + Bu[n] -% -% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ -% -% -% Arguments: -% approx_prediction: if 1 then the exponential map is approximated with a -% first order taylor approximation. has at the moment not a big influence -% (just 1st or 2nd order approximation) we should change it to rodriquez -% approximation. -% use_inertia_matrix: set to true if you have the inertia matrix J for your -% quadrotor -% xa_apo_k: old state vectotr -% zFlag: if sensor measurement is available [gyro, acc, mag] -% dt: dt in s -% z: measurements [gyro, acc, mag] -% q_rotSpeed: process noise gyro -% q_rotAcc: process noise gyro acceleration -% q_acc: process noise acceleration -% q_mag: process noise magnetometer -% r_gyro: measurement noise gyro -% r_accel: measurement noise accel -% r_mag: measurement noise mag -% J: moment of inertia matrix - - -% Output: -% xa_apo: updated state vectotr -% Pa_apo: updated state covariance matrix -% Rot_matrix: rotation matrix -% eulerAngles: euler angles -% debugOutput: not used - - -%% model specific parameters - - - -% compute once the inverse of the Inertia -persistent Ji; -if isempty(Ji) - Ji=single(inv(J)); -end - -%% init -persistent x_apo -if(isempty(x_apo)) - gyro_init=single([0;0;0]); - gyro_acc_init=single([0;0;0]); - acc_init=single([0;0;-9.81]); - mag_init=single([1;0;0]); - x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]); - -end - -persistent P_apo -if(isempty(P_apo)) - % P_apo = single(eye(NSTATES) * 1000); - P_apo = single(200*ones(12)); -end - -debugOutput = single(zeros(4,1)); - -%% copy the states -wx= x_apo(1); % x body angular rate -wy= x_apo(2); % y body angular rate -wz= x_apo(3); % z body angular rate - -wax= x_apo(4); % x body angular acceleration -way= x_apo(5); % y body angular acceleration -waz= x_apo(6); % z body angular acceleration - -zex= x_apo(7); % x component gravity vector -zey= x_apo(8); % y component gravity vector -zez= x_apo(9); % z component gravity vector - -mux= x_apo(10); % x component magnetic field vector -muy= x_apo(11); % y component magnetic field vector -muz= x_apo(12); % z component magnetic field vector - - - - -%% prediction section -% compute the apriori state estimate from the previous aposteriori estimate -%body angular accelerations -if (use_inertia_matrix==1) - wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; -else - wak =[wax;way;waz]; -end - -%body angular rates -wk =[wx; wy; wz] + dt*wak; - -%derivative of the prediction rotation matrix -O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; - -%prediction of the earth z vector -if (approx_prediction==1) - %e^(Odt)=I+dt*O+dt^2/2!O^2 - % so we do a first order approximation of the exponential map - zek =(O*dt+single(eye(3)))*[zex;zey;zez]; - -else - zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; - %zek =expm2(O*dt)*[zex;zey;zez]; not working because use double - %precision -end - - - -%prediction of the magnetic vector -if (approx_prediction==1) - %e^(Odt)=I+dt*O+dt^2/2!O^2 - % so we do a first order approximation of the exponential map - muk =(O*dt+single(eye(3)))*[mux;muy;muz]; -else - muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; - %muk =expm2(O*dt)*[mux;muy;muz]; not working because use double - %precision -end - -x_apr=[wk;wak;zek;muk]; - -% compute the apriori error covariance estimate from the previous -%aposteriori estimate - -EZ=[0,zez,-zey; - -zez,0,zex; - zey,-zex,0]'; -MA=[0,muz,-muy; - -muz,0,mux; - muy,-mux,0]'; - -E=single(eye(3)); -Z=single(zeros(3)); - -A_lin=[ Z, E, Z, Z - Z, Z, Z, Z - EZ, Z, O, Z - MA, Z, Z, O]; - -A_lin=eye(12)+A_lin*dt; - -%process covariance matrix - -persistent Q -if (isempty(Q)) - Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... - q_rotAcc,q_rotAcc,q_rotAcc,... - q_acc,q_acc,q_acc,... - q_mag,q_mag,q_mag]); -end - -P_apr=A_lin*P_apo*A_lin'+Q; - - -%% update -if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 - -% R=[r_gyro,0,0,0,0,0,0,0,0; -% 0,r_gyro,0,0,0,0,0,0,0; -% 0,0,r_gyro,0,0,0,0,0,0; -% 0,0,0,r_accel,0,0,0,0,0; -% 0,0,0,0,r_accel,0,0,0,0; -% 0,0,0,0,0,r_accel,0,0,0; -% 0,0,0,0,0,0,r_mag,0,0; -% 0,0,0,0,0,0,0,r_mag,0; -% 0,0,0,0,0,0,0,0,r_mag]; - R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; - %observation matrix - %[zw;ze;zmk]; - H_k=[ E, Z, Z, Z; - Z, Z, E, Z; - Z, Z, Z, E]; - - y_k=z(1:9)-H_k*x_apr; - - - %S_k=H_k*P_apr*H_k'+R; - S_k=H_k*P_apr*H_k'; - S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; - K_k=(P_apr*H_k'/(S_k)); - - - x_apo=x_apr+K_k*y_k; - P_apo=(eye(12)-K_k*H_k)*P_apr; -else - if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 - - R=[r_gyro,0,0; - 0,r_gyro,0; - 0,0,r_gyro]; - R_v=[r_gyro,r_gyro,r_gyro]; - %observation matrix - - H_k=[ E, Z, Z, Z]; - - y_k=z(1:3)-H_k(1:3,1:12)*x_apr; - - % S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); - S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; - S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; - K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); - - - x_apo=x_apr+K_k*y_k; - P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; - else - if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 - -% R=[r_gyro,0,0,0,0,0; -% 0,r_gyro,0,0,0,0; -% 0,0,r_gyro,0,0,0; -% 0,0,0,r_accel,0,0; -% 0,0,0,0,r_accel,0; -% 0,0,0,0,0,r_accel]; - - R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; - %observation matrix - - H_k=[ E, Z, Z, Z; - Z, Z, E, Z]; - - y_k=z(1:6)-H_k(1:6,1:12)*x_apr; - - % S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); - S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; - S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; - K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); - - - x_apo=x_apr+K_k*y_k; - P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; - else - if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 -% R=[r_gyro,0,0,0,0,0; -% 0,r_gyro,0,0,0,0; -% 0,0,r_gyro,0,0,0; -% 0,0,0,r_mag,0,0; -% 0,0,0,0,r_mag,0; -% 0,0,0,0,0,r_mag]; - R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; - %observation matrix - - H_k=[ E, Z, Z, Z; - Z, Z, Z, E]; - - y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; - - %S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); - S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; - S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; - K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); - - - x_apo=x_apr+K_k*y_k; - P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; - else - x_apo=x_apr; - P_apo=P_apr; - end - end - end -end - - - -%% euler anglels extraction -z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); -m_n_b = x_apo(10:12)./norm(x_apo(10:12)); - -y_n_b=cross(z_n_b,m_n_b); -y_n_b=y_n_b./norm(y_n_b); - -x_n_b=(cross(y_n_b,z_n_b)); -x_n_b=x_n_b./norm(x_n_b); - - -xa_apo=x_apo; -Pa_apo=P_apo; -% rotation matrix from earth to body system -Rot_matrix=[x_n_b,y_n_b,z_n_b]; - - -phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); -theta=-asin(Rot_matrix(1,3)); -psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); -eulerAngles=[phi;theta;psi]; - diff --git a/src/examples/attitude_estimator_ekf/CMakeLists.txt b/src/examples/attitude_estimator_ekf/CMakeLists.txt deleted file mode 100644 index b3869bf290..0000000000 --- a/src/examples/attitude_estimator_ekf/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ -px4_add_module( - MODULE examples__attitude_estimator_ekf - MAIN attitude_estimator_ekf - STACK_MAIN 1200 - COMPILE_FLAGS - -Wno-float-equal - SRCS - attitude_estimator_ekf_main.cpp - codegen/AttitudeEKF.c - DEPENDS - platforms__common - ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/examples/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/examples/attitude_estimator_ekf/attitudeKalmanfilter.prj deleted file mode 100644 index 9ea5203463..0000000000 --- a/src/examples/attitude_estimator_ekf/attitudeKalmanfilter.prj +++ /dev/null @@ -1,502 +0,0 @@ - - - - - false - false - false - option.WorkingFolder.Project - - option.BuildFolder.Project - - - true - true - true - true - option.GlobalDataSyncMethod.SyncAlways - true - option.DynamicMemoryAllocation.Threshold - 65536 - 200000 - option.FilePartitionMethod.MapMFileToCFile - true - false - - true - false - true - false - - - - - - - - - true - false - 40000 - 100 - option.TargetLang.C - true - 10 - 200 - 4000 - true - 64 - true - true - option.ConstantInputs.CheckValues - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - option.WorkingFolder.Project - - option.BuildFolder.Specified - codegen - - true - false - false - false - option.DynamicMemoryAllocation.Threshold - 65536 - 4000 - false - option.FilePartitionMethod.SingleFile - true - true - false - option.DataTypeReplacement.CBuiltIn - false - true - option.ParenthesesLevel.Nominal - 31 - $M$N - $M$N - $M$N - $M$N - $M$N - $M$N - emxArray_$M$N - emx$M$N - - true - false - true - true - false - true - - - - - - - - - C89/C90 (ANSI) - true - ARM Compatible - ARM Cortex - true - 8 - 16 - 32 - 32 - 64 - 32 - 64 - 32 - 32 - option.HardwareEndianness.Little - true - false - option.HardwareAtomicIntegerSize.Long - option.HardwareAtomicFloatSize.Double - option.HardwareDivisionRounding.Undefined - Generic - MATLAB Host Computer - false - 8 - 16 - 32 - 64 - 64 - 32 - 64 - 64 - 64 - option.HardwareEndianness.Little - true - true - option.HardwareAtomicIntegerSize.Char - option.HardwareAtomicFloatSize.None - option.HardwareDivisionRounding.Zero - Automatically locate an installed toolchain - Faster Builds - - 40000 - 100 - true - option.TargetLang.C - option.CCompilerOptimization.On - - true - false - make_rtw - default_tmf - - 10 - 200 - 4000 - true - 64 - true - true - false - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - /opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a - R2012a - true - t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html - /home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html - true - option.VerificationMode.None - false - ${PROJECT_ROOT}/AttitudeEKF_Test.m - ${PROJECT_ROOT}/AttitudeEKF_Test.m - false - false - 1024 - 2048 - AttitudeEKF_mex - AttitudeEKF - option.target.artifact.lib - option.FixedPointTypeProposalMode.ProposeFractionLengths - numerictype([],16,12) - 0 - true - false - false - false - true - false - - option.FixedPointMode.None - false - - - 16 - 4 - 0 - fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128) - option.FixedPointTypeSource.SimAndDerived - - false - false - false - true - - - - - - - - - true - _fixpt - option.DefaultFixedPointSignedness.Automatic - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - uint8 - false - 1 x 1 - false - - - uint8 - false - 1 x 1 - false - - - uint8 - false - 3 x 1 - false - - - single - false - 1 x 1 - false - - - single - false - 9 x 1 - false - - - single - false - 1 x 1 - false - - - single - false - 1 x 1 - false - - - single - false - 1 x 1 - false - - - single - false - 1 x 1 - false - - - single - false - 1 x 1 - false - - - single - false - 1 x 1 - false - - - single - false - 1 x 1 - false - - - single - false - 3 x 3 - false - - - - - - ${PROJECT_ROOT}/AttitudeEKF_Test.m - - - - /opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a - - - - /opt/matlab/r2013b - - - - - - true - false - false - false - false - false - true - false - 3.16.1-1-ARCH - false - true - glnxa64 - true - - - - diff --git a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp deleted file mode 100755 index 1ff585412e..0000000000 --- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ /dev/null @@ -1,639 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file attitude_estimator_ekf_main.cpp - * - * Extended Kalman Filter for Attitude Estimation. - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Thomas Gubler - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif -#include "codegen/AttitudeEKF.h" -#include "attitude_estimator_ekf_params.h" -#ifdef __cplusplus -} -#endif - -extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ - -/** - * Mainloop of attitude_estimator_ekf. - */ -int attitude_estimator_ekf_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) { - fprintf(stderr, "%s\n", reason); - } - - fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); -} - -int parameters_init(struct attitude_estimator_ekf_param_handles *h) -{ - /* PID parameters */ - h->q0 = param_find("EKF_ATT_V3_Q0"); - h->q1 = param_find("EKF_ATT_V3_Q1"); - h->q2 = param_find("EKF_ATT_V3_Q2"); - h->q3 = param_find("EKF_ATT_V3_Q3"); - - h->r0 = param_find("EKF_ATT_V4_R0"); - h->r1 = param_find("EKF_ATT_V4_R1"); - h->r2 = param_find("EKF_ATT_V4_R2"); - - h->moment_inertia_J[0] = param_find("ATT_J11"); - h->moment_inertia_J[1] = param_find("ATT_J22"); - h->moment_inertia_J[2] = param_find("ATT_J33"); - h->use_moment_inertia = param_find("ATT_J_EN"); - - return OK; -} - -int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) -{ - param_get(h->q0, &(p->q[0])); - param_get(h->q1, &(p->q[1])); - param_get(h->q2, &(p->q[2])); - param_get(h->q3, &(p->q[3])); - - param_get(h->r0, &(p->r[0])); - param_get(h->r1, &(p->r[1])); - param_get(h->r2, &(p->r[2])); - - for (int i = 0; i < 3; i++) { - param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); - } - - param_get(h->use_moment_inertia, &(p->use_moment_inertia)); - - return OK; -} - -/** - * The attitude_estimator_ekf app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to px4_task_spawn_cmd(). - */ -int attitude_estimator_ekf_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage("missing command"); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running\n"); - /* this is not an error */ - return 0; - } - - thread_should_exit = false; - attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 7000, - attitude_estimator_ekf_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)nullptr); - return 0; - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("running"); - return 0; - - } else { - warnx("not started"); - return 1; - } - - return 0; - } - - usage("unrecognized command"); - return 1; -} - -/* - * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) - */ - -/* - * EKF Attitude Estimator main function. - * - * Estimates the attitude recursively once started. - * - * @param argc number of commandline arguments (plus command name) - * @param argv strings containing the arguments - */ -int attitude_estimator_ekf_thread_main(int argc, char *argv[]) -{ - - float dt = 0.005f; -/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ - float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ - float x_aposteriori_k[12]; /**< states */ - float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, - }; /**< init: diagonal matrix with big values */ - - float x_aposteriori[12]; - float P_aposteriori[144]; - - /* output euler angles */ - float euler[3] = {0.0f, 0.0f, 0.0f}; - - float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f - }; /**< init: identity matrix */ - - float debugOutput[4] = { 0.0f }; - - /* Initialize filter */ - AttitudeEKF_initialize(); - - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); - gps.eph = 100000; - gps.epv = 100000; - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - - uint64_t last_data = 0; - uint64_t last_measurement = 0; - uint64_t last_vel_t = 0; - - /* current velocity */ - math::Vector<3> vel; - vel.zero(); - /* previous velocity */ - math::Vector<3> vel_prev; - vel_prev.zero(); - /* actual acceleration (by GPS velocity) in body frame */ - math::Vector<3> acc; - acc.zero(); - /* rotation matrix */ - math::Matrix<3, 3> R; - R.identity(); - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - - /* subscribe to GPS */ - int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); - - /* subscribe to GPS */ - int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); - - /* subscribe to param changes */ - int sub_params = orb_subscribe(ORB_ID(parameter_update)); - - /* subscribe to control mode */ - int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); - - /* subscribe to vision estimate */ - int vision_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude)); - - /* subscribe to mocap data */ - int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); - - /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - - int loopcounter = 0; - - thread_running = true; - - /* keep track of sensor updates */ - uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - - struct attitude_estimator_ekf_params ekf_params; - memset(&ekf_params, 0, sizeof(ekf_params)); - - struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; - - /* initialize parameter handles */ - parameters_init(&ekf_param_handles); - - bool initialized = false; - - float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - - /* magnetic declination, in radians */ - float mag_decl = 0.0f; - - /* rotation matrix for magnetic declination */ - math::Matrix<3, 3> R_decl; - R_decl.identity(); - - struct vehicle_attitude_s vision {}; - struct att_pos_mocap_s mocap {}; - - /* register the perf counter */ - perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); - - /* Main loop*/ - while (!thread_should_exit) { - - px4_pollfd_struct_t fds[2]; - fds[0].fd = sub_raw; - fds[0].events = POLLIN; - fds[1].fd = sub_params; - fds[1].events = POLLIN; - int ret = px4_poll(fds, 2, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - - if (!control_mode.flag_system_hil_enabled) { - warnx("WARNING: Not getting sensor data - sensor app running?"); - } - - } else { - - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), sub_params, &update); - - /* update parameters */ - parameters_update(&ekf_param_handles, &ekf_params); - } - - /* only run filter if sensor values changed */ - if (fds[0].revents & POLLIN) { - - /* get latest measurements */ - orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - - bool gps_updated; - orb_check(sub_gps, &gps_updated); - if (gps_updated) { - orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); - - if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { - mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); - - /* update mag declination rotation matrix */ - R_decl.from_euler(0.0f, 0.0f, mag_decl); - } - } - - bool global_pos_updated; - orb_check(sub_global_pos, &global_pos_updated); - if (global_pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); - } - - if (!initialized) { - // XXX disabling init for now - initialized = true; - - // gyro_offsets[0] += raw.gyro_rad[0]; - // gyro_offsets[1] += raw.gyro_rad[1]; - // gyro_offsets[2] += raw.gyro_rad[2]; - // offset_count++; - - // if (hrt_absolute_time() - start_time > 3000000LL) { - // initialized = true; - // gyro_offsets[0] /= offset_count; - // gyro_offsets[1] /= offset_count; - // gyro_offsets[2] /= offset_count; - // } - - } else { - - perf_begin(ekf_loop_perf); - - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; - - /* Fill in gyro measurements */ - if (sensor_last_timestamp[0] != raw.timestamp) { - update_vect[0] = 1; - // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; - } - - z_k[0] = raw.gyro_rad[0] - gyro_offsets[0]; - z_k[1] = raw.gyro_rad[1] - gyro_offsets[1]; - z_k[2] = raw.gyro_rad[2] - gyro_offsets[2]; - - /* update accelerometer measurements */ - if (sensor_last_timestamp[1] != raw.timestamp + raw.accelerometer_timestamp_relative) { - update_vect[1] = 1; - // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp + raw.accelerometer_timestamp_relative; - } - - hrt_abstime vel_t = 0; - bool vel_valid = false; - if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { - vel_valid = true; - if (global_pos_updated) { - vel_t = global_pos.timestamp; - vel(0) = global_pos.vel_n; - vel(1) = global_pos.vel_e; - vel(2) = global_pos.vel_d; - } - } - - if (vel_valid) { - /* velocity is valid */ - if (vel_t != 0) { - /* velocity updated */ - if (last_vel_t != 0 && vel_t != last_vel_t) { - float vel_dt = (vel_t - last_vel_t) / 1000000.0f; - /* calculate acceleration in body frame */ - acc = R.transposed() * ((vel - vel_prev) / vel_dt); - } - last_vel_t = vel_t; - vel_prev = vel; - } - - } else { - /* velocity is valid, reset acceleration */ - acc.zero(); - vel_prev.zero(); - last_vel_t = 0; - } - - z_k[3] = raw.accelerometer_m_s2[0] - acc(0); - z_k[4] = raw.accelerometer_m_s2[1] - acc(1); - z_k[5] = raw.accelerometer_m_s2[2] - acc(2); - - /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.timestamp + raw.magnetometer_timestamp_relative && - /* check that the mag vector is > 0 */ - fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + - raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + - raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { - update_vect[2] = 1; - // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp + raw.magnetometer_timestamp_relative; - } - - bool vision_updated = false; - orb_check(vision_sub, &vision_updated); - - bool mocap_updated = false; - orb_check(mocap_sub, &mocap_updated); - - if (vision_updated) { - orb_copy(ORB_ID(vehicle_vision_attitude), vision_sub, &vision); - } - - if (mocap_updated) { - orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap); - } - - if (mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000)) { - - math::Quaternion q(mocap.q); - math::Matrix<3, 3> Rmoc = q.to_dcm(); - - math::Vector<3> v(1.0f, 0.0f, 0.4f); - - math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw - // Rrw * Vw = vn. This way we have consistency - z_k[6] = vn(0); - z_k[7] = vn(1); - z_k[8] = vn(2); - } else if (vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000)) { - - math::Quaternion q(vision.q); - math::Matrix<3, 3> Rvis = q.to_dcm(); - - math::Vector<3> v(1.0f, 0.0f, 0.4f); - - math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw - // Rrw * Vw = vn. This way we have consistency - z_k[6] = vn(0); - z_k[7] = vn(1); - z_k[8] = vn(2); - } else { - z_k[6] = raw.magnetometer_ga[0]; - z_k[7] = raw.magnetometer_ga[1]; - z_k[8] = raw.magnetometer_ga[2]; - } - - static bool const_initialized = false; - - /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.001f) { - dt = 0.005f; - parameters_update(&ekf_param_handles, &ekf_params); - - /* update mag declination rotation matrix */ - if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { - mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); - - } - - /* update mag declination rotation matrix */ - R_decl.from_euler(0.0f, 0.0f, mag_decl); - - x_aposteriori_k[0] = z_k[0]; - x_aposteriori_k[1] = z_k[1]; - x_aposteriori_k[2] = z_k[2]; - x_aposteriori_k[3] = 0.0f; - x_aposteriori_k[4] = 0.0f; - x_aposteriori_k[5] = 0.0f; - x_aposteriori_k[6] = z_k[3]; - x_aposteriori_k[7] = z_k[4]; - x_aposteriori_k[8] = z_k[5]; - x_aposteriori_k[9] = z_k[6]; - x_aposteriori_k[10] = z_k[7]; - x_aposteriori_k[11] = z_k[8]; - - const_initialized = true; - } - - /* do not execute the filter if not initialized */ - if (!const_initialized) { - continue; - } - - /* Call the estimator */ - AttitudeEKF(false, // approx_prediction - (unsigned char)ekf_params.use_moment_inertia, - update_vect, - dt, - z_k, - ekf_params.q[0], // q_rotSpeed, - ekf_params.q[1], // q_rotAcc - ekf_params.q[2], // q_acc - ekf_params.q[3], // q_mag - ekf_params.r[0], // r_gyro - ekf_params.r[1], // r_accel - ekf_params.r[2], // r_mag - ekf_params.moment_inertia_J, - x_aposteriori, - P_aposteriori, - Rot_matrix, - euler, - debugOutput); - - /* swap values for next iteration, check for fatal inputs */ - if (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_ISFINITE(euler[2])) { - memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); - memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); - - } else { - /* due to inputs or numerical failure the output is invalid, skip it */ - continue; - } - - if (last_data > 0 && raw.timestamp - last_data > 30000) { - warnx("sensor data missed! (%llu)\n", static_cast(raw.timestamp - last_data)); - } - - last_data = raw.timestamp; - - /* send out */ - att.timestamp = raw.timestamp; - - - att.rollspeed = x_aposteriori[0]; - att.pitchspeed = x_aposteriori[1]; - att.yawspeed = x_aposteriori[2]; - - /* magnetic declination */ - matrix::Dcmf Ro(&Rot_matrix[0]); - matrix::Dcmf R_declination(&R_decl.data[0][0]); - matrix::Quatf q = matrix::Quatf(R_declination * Ro); - - memcpy(&att.q[0],&q._data[0],sizeof(att.q)); - - if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1]) - && PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) { - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - - } else { - PX4_ERR("ERR: NaN estimate!"); - } - - perf_end(ekf_loop_perf); - } - } - } - - loopcounter++; - } - - thread_running = false; - - return 0; -} diff --git a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.c deleted file mode 100755 index c10f871022..0000000000 --- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ /dev/null @@ -1,130 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file attitude_estimator_ekf_params.c - * - * Parameters for EKF filter - */ - -#include "attitude_estimator_ekf_params.h" -#include -#include - -/* Extended Kalman Filter covariances */ - - -/** - * Body angular rate process noise - * - * @group Attitude EKF estimator - */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); - -/** - * Body angular acceleration process noise - * - * @group Attitude EKF estimator - */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); - -/** - * Acceleration process noise - * - * @group Attitude EKF estimator - */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); - -/** - * Magnet field vector process noise - * - * @group Attitude EKF estimator - */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); - -/** - * Gyro measurement noise - * - * @group Attitude EKF estimator - */ -PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); - -/** - * Accel measurement noise - * - * @group Attitude EKF estimator - */ -PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); - -/** - * Mag measurement noise - * - * @group Attitude EKF estimator - */ -PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); - -/** - * Moment of inertia matrix diagonal entry (1, 1) - * - * @group Attitude EKF estimator - * @unit kg*m^2 - */ -PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); - -/** - * Moment of inertia matrix diagonal entry (2, 2) - * - * @group Attitude EKF estimator - * @unit kg*m^2 - */ -PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); - -/** - * Moment of inertia matrix diagonal entry (3, 3) - * - * @group Attitude EKF estimator - * @unit kg*m^2 - */ -PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); - -/** - * Moment of inertia enabled in estimator - * - * If set to != 0 the moment of inertia will be used in the estimator - * - * @group Attitude EKF estimator - * @boolean - */ -PARAM_DEFINE_INT32(ATT_J_EN, 0); diff --git a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.h deleted file mode 100755 index 5d3b6b2440..0000000000 --- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ /dev/null @@ -1,75 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file attitude_estimator_ekf_params.h - * - * Parameters for EKF filter - */ - -#include - -struct attitude_estimator_ekf_params { - float r[3]; - float q[4]; - float moment_inertia_J[9]; - int32_t use_moment_inertia; - float roll_off; - float pitch_off; - float yaw_off; - float mag_decl; - int acc_comp; -}; - -struct attitude_estimator_ekf_param_handles { - param_t r0, r1, r2; - param_t q0, q1, q2, q3; - param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */ - param_t use_moment_inertia; - param_t mag_decl; - param_t acc_comp; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct attitude_estimator_ekf_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p); diff --git a/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.c deleted file mode 100644 index 25bdde5cf1..0000000000 --- a/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.c +++ /dev/null @@ -1,1456 +0,0 @@ -/* - * AttitudeEKF.c - * - * Code generation for function 'AttitudeEKF' - * - * C source code generated on: Thu Aug 21 11:17:28 2014 - * - */ - -/* Include files */ -#include "AttitudeEKF.h" - -/* Variable Definitions */ -static float Ji[9]; -static boolean_T Ji_not_empty; -static float x_apo[12]; -static float P_apo[144]; -static float Q[144]; -static boolean_T Q_not_empty; - -/* Function Declarations */ -static void AttitudeEKF_init(void); -static void b_mrdivide(const float A[72], const float B[36], float y[72]); -static void inv(const float x[9], float y[9]); -static void mrdivide(const float A[108], const float B[81], float y[108]); -static float norm(const float x[3]); - -/* Function Definitions */ -static void AttitudeEKF_init(void) -{ - int i; - static const float fv5[12] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - -9.81F, 1.0F, 0.0F, 0.0F }; - - for (i = 0; i < 12; i++) { - x_apo[i] = fv5[i]; - } - - for (i = 0; i < 144; i++) { - P_apo[i] = 200.0F; - } -} - -/* - * - */ -static void b_mrdivide(const float A[72], const float B[36], float y[72]) -{ - float b_A[36]; - signed char ipiv[6]; - int i1; - int iy; - int j; - int c; - int ix; - float temp; - int k; - float s; - int jy; - int ijA; - float Y[72]; - for (i1 = 0; i1 < 6; i1++) { - for (iy = 0; iy < 6; iy++) { - b_A[iy + 6 * i1] = B[i1 + 6 * iy]; - } - - ipiv[i1] = (signed char)(1 + i1); - } - - for (j = 0; j < 5; j++) { - c = j * 7; - iy = 0; - ix = c; - temp = (real32_T)fabs(b_A[c]); - for (k = 2; k <= 6 - j; k++) { - ix++; - s = (real32_T)fabs(b_A[ix]); - if (s > temp) { - iy = k - 1; - temp = s; - } - } - - if (b_A[c + iy] != 0.0F) { - if (iy != 0) { - ipiv[j] = (signed char)((j + iy) + 1); - ix = j; - iy += j; - for (k = 0; k < 6; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 6; - iy += 6; - } - } - - i1 = (c - j) + 6; - for (jy = c + 1; jy + 1 <= i1; jy++) { - b_A[jy] /= b_A[c]; - } - } - - iy = c; - jy = c + 6; - for (k = 1; k <= 5 - j; k++) { - temp = b_A[jy]; - if (b_A[jy] != 0.0F) { - ix = c + 1; - i1 = (iy - j) + 12; - for (ijA = 7 + iy; ijA + 1 <= i1; ijA++) { - b_A[ijA] += b_A[ix] * -temp; - ix++; - } - } - - jy += 6; - iy += 6; - } - } - - for (i1 = 0; i1 < 12; i1++) { - for (iy = 0; iy < 6; iy++) { - Y[iy + 6 * i1] = A[i1 + 12 * iy]; - } - } - - for (jy = 0; jy < 6; jy++) { - if (ipiv[jy] != jy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[jy + 6 * j]; - Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; - Y[(ipiv[jy] + 6 * j) - 1] = temp; - } - } - } - - for (j = 0; j < 12; j++) { - c = 6 * j; - for (k = 0; k < 6; k++) { - iy = 6 * k; - if (Y[k + c] != 0.0F) { - for (jy = k + 2; jy < 7; jy++) { - Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; - } - } - } - } - - for (j = 0; j < 12; j++) { - c = 6 * j; - for (k = 5; k > -1; k += -1) { - iy = 6 * k; - if (Y[k + c] != 0.0F) { - Y[k + c] /= b_A[k + iy]; - for (jy = 0; jy + 1 <= k; jy++) { - Y[jy + c] -= Y[k + c] * b_A[jy + iy]; - } - } - } - } - - for (i1 = 0; i1 < 6; i1++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * i1] = Y[i1 + 6 * iy]; - } - } -} - -/* - * - */ -static void inv(const float x[9], float y[9]) -{ - float b_x[9]; - int p1; - int p2; - int p3; - float absx11; - float absx21; - float absx31; - int itmp; - for (p1 = 0; p1 < 9; p1++) { - b_x[p1] = x[p1]; - } - - p1 = 0; - p2 = 3; - p3 = 6; - absx11 = (real32_T)fabs(x[0]); - absx21 = (real32_T)fabs(x[1]); - absx31 = (real32_T)fabs(x[2]); - if ((absx21 > absx11) && (absx21 > absx31)) { - p1 = 3; - p2 = 0; - b_x[0] = x[1]; - b_x[1] = x[0]; - b_x[3] = x[4]; - b_x[4] = x[3]; - b_x[6] = x[7]; - b_x[7] = x[6]; - } else { - if (absx31 > absx11) { - p1 = 6; - p3 = 0; - b_x[0] = x[2]; - b_x[2] = x[0]; - b_x[3] = x[5]; - b_x[5] = x[3]; - b_x[6] = x[8]; - b_x[8] = x[6]; - } - } - - absx11 = b_x[1] / b_x[0]; - b_x[1] /= b_x[0]; - absx21 = b_x[2] / b_x[0]; - b_x[2] /= b_x[0]; - b_x[4] -= absx11 * b_x[3]; - b_x[5] -= absx21 * b_x[3]; - b_x[7] -= absx11 * b_x[6]; - b_x[8] -= absx21 * b_x[6]; - if ((real32_T)fabs(b_x[5]) > (real32_T)fabs(b_x[4])) { - itmp = p2; - p2 = p3; - p3 = itmp; - b_x[1] = absx21; - b_x[2] = absx11; - absx11 = b_x[4]; - b_x[4] = b_x[5]; - b_x[5] = absx11; - absx11 = b_x[7]; - b_x[7] = b_x[8]; - b_x[8] = absx11; - } - - absx11 = b_x[5] / b_x[4]; - b_x[5] /= b_x[4]; - b_x[8] -= absx11 * b_x[7]; - absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8]; - absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4]; - y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0]; - y[p1 + 1] = absx21; - y[p1 + 2] = absx11; - absx11 = -b_x[5] / b_x[8]; - absx21 = (1.0F - b_x[7] * absx11) / b_x[4]; - y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; - y[p2 + 1] = absx21; - y[p2 + 2] = absx11; - absx11 = 1.0F / b_x[8]; - absx21 = -b_x[7] * absx11 / b_x[4]; - y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; - y[p3 + 1] = absx21; - y[p3 + 2] = absx11; -} - -/* - * - */ -static void mrdivide(const float A[108], const float B[81], float y[108]) -{ - float b_A[81]; - signed char ipiv[9]; - int i0; - int iy; - int j; - int c; - int ix; - float temp; - int k; - float s; - int jy; - int ijA; - float Y[108]; - for (i0 = 0; i0 < 9; i0++) { - for (iy = 0; iy < 9; iy++) { - b_A[iy + 9 * i0] = B[i0 + 9 * iy]; - } - - ipiv[i0] = (signed char)(1 + i0); - } - - for (j = 0; j < 8; j++) { - c = j * 10; - iy = 0; - ix = c; - temp = (real32_T)fabs(b_A[c]); - for (k = 2; k <= 9 - j; k++) { - ix++; - s = (real32_T)fabs(b_A[ix]); - if (s > temp) { - iy = k - 1; - temp = s; - } - } - - if (b_A[c + iy] != 0.0F) { - if (iy != 0) { - ipiv[j] = (signed char)((j + iy) + 1); - ix = j; - iy += j; - for (k = 0; k < 9; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 9; - iy += 9; - } - } - - i0 = (c - j) + 9; - for (jy = c + 1; jy + 1 <= i0; jy++) { - b_A[jy] /= b_A[c]; - } - } - - iy = c; - jy = c + 9; - for (k = 1; k <= 8 - j; k++) { - temp = b_A[jy]; - if (b_A[jy] != 0.0F) { - ix = c + 1; - i0 = (iy - j) + 18; - for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) { - b_A[ijA] += b_A[ix] * -temp; - ix++; - } - } - - jy += 9; - iy += 9; - } - } - - for (i0 = 0; i0 < 12; i0++) { - for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * i0] = A[i0 + 12 * iy]; - } - } - - for (jy = 0; jy < 9; jy++) { - if (ipiv[jy] != jy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[jy + 9 * j]; - Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; - Y[(ipiv[jy] + 9 * j) - 1] = temp; - } - } - } - - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 0; k < 9; k++) { - iy = 9 * k; - if (Y[k + c] != 0.0F) { - for (jy = k + 2; jy < 10; jy++) { - Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; - } - } - } - } - - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 8; k > -1; k += -1) { - iy = 9 * k; - if (Y[k + c] != 0.0F) { - Y[k + c] /= b_A[k + iy]; - for (jy = 0; jy + 1 <= k; jy++) { - Y[jy + c] -= Y[k + c] * b_A[jy + iy]; - } - } - } - } - - for (i0 = 0; i0 < 9; i0++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * i0] = Y[i0 + 9 * iy]; - } - } -} - -/* - * - */ -static float norm(const float x[3]) -{ - float y; - float scale; - int k; - float absxk; - float t; - y = 0.0F; - scale = 1.17549435E-38F; - for (k = 0; k < 3; k++) { - absxk = (real32_T)fabs(x[k]); - if (absxk > scale) { - t = scale / absxk; - y = 1.0F + y * t * t; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - return scale * (real32_T)sqrt(y); -} - -/* - * function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... - * = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) - */ -void AttitudeEKF(unsigned char approx_prediction, unsigned char - use_inertia_matrix, const unsigned char zFlag[3], float dt, - const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, - float q_mag, float r_gyro, float r_accel, float r_mag, const - float J[9], float xa_apo[12], float Pa_apo[144], float - Rot_matrix[9], float eulerAngles[3], float debugOutput[4]) -{ - int i; - float fv0[3]; - int r2; - float zek[3]; - float muk[3]; - float b_muk[3]; - float c_muk[3]; - float fv1[3]; - float wak[3]; - float O[9]; - float b_O[9]; - static const signed char iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - - float fv2[3]; - float maxval; - int r1; - float fv3[9]; - float fv4[3]; - float x_apr[12]; - signed char I[144]; - static float A_lin[144]; - static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - static float b_A_lin[144]; - float v[12]; - static float P_apr[144]; - float a[108]; - static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1, 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - float S_k[81]; - static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - float b_r_gyro[9]; - float K_k[108]; - float b_S_k[36]; - static const signed char c_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 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 const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - float c_r_gyro[3]; - float B[36]; - int r3; - float a21; - float Y[36]; - float d_a[72]; - static const signed char e_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0 }; - - static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0 }; - - float d_r_gyro[6]; - float c_S_k[6]; - float b_K_k[72]; - static const signed char f_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, - 1, 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1 }; - - static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1 }; - - float b_z[6]; - - /* LQG Position Estimator and Controller */ - /* Observer: */ - /* x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) */ - /* x[n+1|n] = Ax[n|n] + Bu[n] */ - /* */ - /* $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ */ - /* */ - /* */ - /* Arguments: */ - /* approx_prediction: if 1 then the exponential map is approximated with a */ - /* first order taylor approximation. has at the moment not a big influence */ - /* (just 1st or 2nd order approximation) we should change it to rodriquez */ - /* approximation. */ - /* use_inertia_matrix: set to true if you have the inertia matrix J for your */ - /* quadrotor */ - /* xa_apo_k: old state vectotr */ - /* zFlag: if sensor measurement is available [gyro, acc, mag] */ - /* dt: dt in s */ - /* z: measurements [gyro, acc, mag] */ - /* q_rotSpeed: process noise gyro */ - /* q_rotAcc: process noise gyro acceleration */ - /* q_acc: process noise acceleration */ - /* q_mag: process noise magnetometer */ - /* r_gyro: measurement noise gyro */ - /* r_accel: measurement noise accel */ - /* r_mag: measurement noise mag */ - /* J: moment of inertia matrix */ - /* Output: */ - /* xa_apo: updated state vectotr */ - /* Pa_apo: updated state covariance matrix */ - /* Rot_matrix: rotation matrix */ - /* eulerAngles: euler angles */ - /* debugOutput: not used */ - /* % model specific parameters */ - /* compute once the inverse of the Inertia */ - /* 'AttitudeEKF:48' if isempty(Ji) */ - if (!Ji_not_empty) { - /* 'AttitudeEKF:49' Ji=single(inv(J)); */ - inv(J, Ji); - Ji_not_empty = TRUE; - } - - /* % init */ - /* 'AttitudeEKF:54' if(isempty(x_apo)) */ - /* 'AttitudeEKF:64' if(isempty(P_apo)) */ - /* 'AttitudeEKF:69' debugOutput = single(zeros(4,1)); */ - for (i = 0; i < 4; i++) { - debugOutput[i] = 0.0F; - } - - /* % copy the states */ - /* 'AttitudeEKF:72' wx= x_apo(1); */ - /* x body angular rate */ - /* 'AttitudeEKF:73' wy= x_apo(2); */ - /* y body angular rate */ - /* 'AttitudeEKF:74' wz= x_apo(3); */ - /* z body angular rate */ - /* 'AttitudeEKF:76' wax= x_apo(4); */ - /* x body angular acceleration */ - /* 'AttitudeEKF:77' way= x_apo(5); */ - /* y body angular acceleration */ - /* 'AttitudeEKF:78' waz= x_apo(6); */ - /* z body angular acceleration */ - /* 'AttitudeEKF:80' zex= x_apo(7); */ - /* x component gravity vector */ - /* 'AttitudeEKF:81' zey= x_apo(8); */ - /* y component gravity vector */ - /* 'AttitudeEKF:82' zez= x_apo(9); */ - /* z component gravity vector */ - /* 'AttitudeEKF:84' mux= x_apo(10); */ - /* x component magnetic field vector */ - /* 'AttitudeEKF:85' muy= x_apo(11); */ - /* y component magnetic field vector */ - /* 'AttitudeEKF:86' muz= x_apo(12); */ - /* z component magnetic field vector */ - /* % prediction section */ - /* compute the apriori state estimate from the previous aposteriori estimate */ - /* body angular accelerations */ - /* 'AttitudeEKF:94' if (use_inertia_matrix==1) */ - if (use_inertia_matrix == 1) { - /* 'AttitudeEKF:95' wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; */ - fv0[0] = x_apo[3]; - fv0[1] = x_apo[4]; - fv0[2] = x_apo[5]; - for (r2 = 0; r2 < 3; r2++) { - zek[r2] = 0.0F; - for (i = 0; i < 3; i++) { - zek[r2] += J[r2 + 3 * i] * fv0[i]; - } - } - - muk[0] = x_apo[3]; - muk[1] = x_apo[4]; - muk[2] = x_apo[5]; - b_muk[0] = x_apo[4] * zek[2] - x_apo[5] * zek[1]; - b_muk[1] = x_apo[5] * zek[0] - x_apo[3] * zek[2]; - b_muk[2] = x_apo[3] * zek[1] - x_apo[4] * zek[0]; - for (r2 = 0; r2 < 3; r2++) { - c_muk[r2] = -b_muk[r2]; - } - - fv1[0] = x_apo[3]; - fv1[1] = x_apo[4]; - fv1[2] = x_apo[5]; - for (r2 = 0; r2 < 3; r2++) { - fv0[r2] = 0.0F; - for (i = 0; i < 3; i++) { - fv0[r2] += Ji[r2 + 3 * i] * c_muk[i]; - } - - wak[r2] = fv1[r2] + fv0[r2] * dt; - } - } else { - /* 'AttitudeEKF:96' else */ - /* 'AttitudeEKF:97' wak =[wax;way;waz]; */ - wak[0] = x_apo[3]; - wak[1] = x_apo[4]; - wak[2] = x_apo[5]; - } - - /* body angular rates */ - /* 'AttitudeEKF:101' wk =[wx; wy; wz] + dt*wak; */ - /* derivative of the prediction rotation matrix */ - /* 'AttitudeEKF:104' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ - O[0] = 0.0F; - O[1] = -x_apo[2]; - O[2] = x_apo[1]; - O[3] = x_apo[2]; - O[4] = 0.0F; - O[5] = -x_apo[0]; - O[6] = -x_apo[1]; - O[7] = x_apo[0]; - O[8] = 0.0F; - - /* prediction of the earth z vector */ - /* 'AttitudeEKF:107' if (approx_prediction==1) */ - if (approx_prediction == 1) { - /* e^(Odt)=I+dt*O+dt^2/2!O^2 */ - /* so we do a first order approximation of the exponential map */ - /* 'AttitudeEKF:110' zek =(O*dt+single(eye(3)))*[zex;zey;zez]; */ - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; - } - } - - fv2[0] = x_apo[6]; - fv2[1] = x_apo[7]; - fv2[2] = x_apo[8]; - for (r2 = 0; r2 < 3; r2++) { - zek[r2] = 0.0F; - for (i = 0; i < 3; i++) { - zek[r2] += b_O[r2 + 3 * i] * fv2[i]; - } - } - } else { - /* 'AttitudeEKF:112' else */ - /* 'AttitudeEKF:113' zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; */ - maxval = dt * dt / 2.0F; - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - b_O[r2 + 3 * i] = 0.0F; - for (r1 = 0; r1 < 3; r1++) { - b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; - } - } - } - - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval - * b_O[i + 3 * r2]; - } - } - - fv2[0] = x_apo[6]; - fv2[1] = x_apo[7]; - fv2[2] = x_apo[8]; - for (r2 = 0; r2 < 3; r2++) { - zek[r2] = 0.0F; - for (i = 0; i < 3; i++) { - zek[r2] += fv3[r2 + 3 * i] * fv2[i]; - } - } - - /* zek =expm2(O*dt)*[zex;zey;zez]; not working because use double */ - /* precision */ - } - - /* prediction of the magnetic vector */ - /* 'AttitudeEKF:121' if (approx_prediction==1) */ - if (approx_prediction == 1) { - /* e^(Odt)=I+dt*O+dt^2/2!O^2 */ - /* so we do a first order approximation of the exponential map */ - /* 'AttitudeEKF:124' muk =(O*dt+single(eye(3)))*[mux;muy;muz]; */ - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; - } - } - - fv4[0] = x_apo[9]; - fv4[1] = x_apo[10]; - fv4[2] = x_apo[11]; - for (r2 = 0; r2 < 3; r2++) { - muk[r2] = 0.0F; - for (i = 0; i < 3; i++) { - muk[r2] += b_O[r2 + 3 * i] * fv4[i]; - } - } - } else { - /* 'AttitudeEKF:125' else */ - /* 'AttitudeEKF:126' muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; */ - maxval = dt * dt / 2.0F; - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - b_O[r2 + 3 * i] = 0.0F; - for (r1 = 0; r1 < 3; r1++) { - b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; - } - } - } - - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval - * b_O[i + 3 * r2]; - } - } - - fv4[0] = x_apo[9]; - fv4[1] = x_apo[10]; - fv4[2] = x_apo[11]; - for (r2 = 0; r2 < 3; r2++) { - muk[r2] = 0.0F; - for (i = 0; i < 3; i++) { - muk[r2] += fv3[r2 + 3 * i] * fv4[i]; - } - } - - /* muk =expm2(O*dt)*[mux;muy;muz]; not working because use double */ - /* precision */ - } - - /* 'AttitudeEKF:131' x_apr=[wk;wak;zek;muk]; */ - x_apr[0] = x_apo[0] + dt * wak[0]; - x_apr[1] = x_apo[1] + dt * wak[1]; - x_apr[2] = x_apo[2] + dt * wak[2]; - for (i = 0; i < 3; i++) { - x_apr[i + 3] = wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apr[i + 6] = zek[i]; - } - - for (i = 0; i < 3; i++) { - x_apr[i + 9] = muk[i]; - } - - /* compute the apriori error covariance estimate from the previous */ - /* aposteriori estimate */ - /* 'AttitudeEKF:136' EZ=[0,zez,-zey; */ - /* 'AttitudeEKF:137' -zez,0,zex; */ - /* 'AttitudeEKF:138' zey,-zex,0]'; */ - /* 'AttitudeEKF:139' MA=[0,muz,-muy; */ - /* 'AttitudeEKF:140' -muz,0,mux; */ - /* 'AttitudeEKF:141' muy,-mux,0]'; */ - /* 'AttitudeEKF:143' E=single(eye(3)); */ - /* 'AttitudeEKF:144' Z=single(zeros(3)); */ - /* 'AttitudeEKF:146' A_lin=[ Z, E, Z, Z */ - /* 'AttitudeEKF:147' Z, Z, Z, Z */ - /* 'AttitudeEKF:148' EZ, Z, O, Z */ - /* 'AttitudeEKF:149' MA, Z, Z, O]; */ - /* 'AttitudeEKF:151' A_lin=eye(12)+A_lin*dt; */ - memset(&I[0], 0, 144U * sizeof(signed char)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1; - for (r2 = 0; r2 < 3; r2++) { - A_lin[r2 + 12 * i] = iv1[r2 + 3 * i]; - } - - for (r2 = 0; r2 < 3; r2++) { - A_lin[(r2 + 12 * i) + 3] = 0.0F; - } - } - - A_lin[6] = 0.0F; - A_lin[7] = x_apo[8]; - A_lin[8] = -x_apo[7]; - A_lin[18] = -x_apo[8]; - A_lin[19] = 0.0F; - A_lin[20] = x_apo[6]; - A_lin[30] = x_apo[7]; - A_lin[31] = -x_apo[6]; - A_lin[32] = 0.0F; - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - A_lin[(i + 12 * (r2 + 3)) + 6] = 0.0F; - } - } - - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - A_lin[(i + 12 * (r2 + 6)) + 6] = O[i + 3 * r2]; - } - } - - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - A_lin[(i + 12 * (r2 + 9)) + 6] = 0.0F; - } - } - - A_lin[9] = 0.0F; - A_lin[10] = x_apo[11]; - A_lin[11] = -x_apo[10]; - A_lin[21] = -x_apo[11]; - A_lin[22] = 0.0F; - A_lin[23] = x_apo[9]; - A_lin[33] = x_apo[10]; - A_lin[34] = -x_apo[9]; - A_lin[35] = 0.0F; - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - A_lin[(i + 12 * (r2 + 3)) + 9] = 0.0F; - } - } - - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - A_lin[(i + 12 * (r2 + 6)) + 9] = 0.0F; - } - } - - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - A_lin[(i + 12 * (r2 + 9)) + 9] = O[i + 3 * r2]; - } - } - - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - b_A_lin[i + 12 * r2] = (float)I[i + 12 * r2] + A_lin[i + 12 * r2] * dt; - } - } - - /* process covariance matrix */ - /* 'AttitudeEKF:156' if (isempty(Q)) */ - if (!Q_not_empty) { - /* 'AttitudeEKF:157' Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... */ - /* 'AttitudeEKF:158' q_rotAcc,q_rotAcc,q_rotAcc,... */ - /* 'AttitudeEKF:159' q_acc,q_acc,q_acc,... */ - /* 'AttitudeEKF:160' q_mag,q_mag,q_mag]); */ - v[0] = q_rotSpeed; - v[1] = q_rotSpeed; - v[2] = q_rotSpeed; - v[3] = q_rotAcc; - v[4] = q_rotAcc; - v[5] = q_rotAcc; - v[6] = q_acc; - v[7] = q_acc; - v[8] = q_acc; - v[9] = q_mag; - v[10] = q_mag; - v[11] = q_mag; - memset(&Q[0], 0, 144U * sizeof(float)); - for (i = 0; i < 12; i++) { - Q[i + 12 * i] = v[i]; - } - - Q_not_empty = TRUE; - } - - /* 'AttitudeEKF:163' P_apr=A_lin*P_apo*A_lin'+Q; */ - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - A_lin[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - A_lin[r2 + 12 * i] += b_A_lin[r2 + 12 * r1] * P_apo[r1 + 12 * i]; - } - } - } - - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - maxval += A_lin[r2 + 12 * r1] * b_A_lin[i + 12 * r1]; - } - - P_apr[r2 + 12 * i] = maxval + Q[r2 + 12 * i]; - } - } - - /* % update */ - /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */ - if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) { - /* R=[r_gyro,0,0,0,0,0,0,0,0; */ - /* 0,r_gyro,0,0,0,0,0,0,0; */ - /* 0,0,r_gyro,0,0,0,0,0,0; */ - /* 0,0,0,r_accel,0,0,0,0,0; */ - /* 0,0,0,0,r_accel,0,0,0,0; */ - /* 0,0,0,0,0,r_accel,0,0,0; */ - /* 0,0,0,0,0,0,r_mag,0,0; */ - /* 0,0,0,0,0,0,0,r_mag,0; */ - /* 0,0,0,0,0,0,0,0,r_mag]; */ - /* 'AttitudeEKF:178' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; */ - /* observation matrix */ - /* [zw;ze;zmk]; */ - /* 'AttitudeEKF:181' H_k=[ E, Z, Z, Z; */ - /* 'AttitudeEKF:182' Z, Z, E, Z; */ - /* 'AttitudeEKF:183' Z, Z, Z, E]; */ - /* 'AttitudeEKF:185' y_k=z(1:9)-H_k*x_apr; */ - /* S_k=H_k*P_apr*H_k'+R; */ - /* 'AttitudeEKF:189' S_k=H_k*P_apr*H_k'; */ - for (r2 = 0; r2 < 9; r2++) { - for (i = 0; i < 12; i++) { - a[r2 + 9 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - a[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i]; - } - } - - for (i = 0; i < 9; i++) { - S_k[r2 + 9 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - S_k[r2 + 9 * i] += a[r2 + 9 * r1] * (float)b[r1 + 12 * i]; - } - } - } - - /* 'AttitudeEKF:190' S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; */ - b_r_gyro[0] = r_gyro; - b_r_gyro[1] = r_gyro; - b_r_gyro[2] = r_gyro; - b_r_gyro[3] = r_accel; - b_r_gyro[4] = r_accel; - b_r_gyro[5] = r_accel; - b_r_gyro[6] = r_mag; - b_r_gyro[7] = r_mag; - b_r_gyro[8] = r_mag; - for (r2 = 0; r2 < 9; r2++) { - b_O[r2] = S_k[10 * r2] + b_r_gyro[r2]; - } - - for (r2 = 0; r2 < 9; r2++) { - S_k[10 * r2] = b_O[r2]; - } - - /* 'AttitudeEKF:191' K_k=(P_apr*H_k'/(S_k)); */ - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 9; i++) { - a[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i]; - } - } - } - - mrdivide(a, S_k, K_k); - - /* 'AttitudeEKF:194' x_apo=x_apr+K_k*y_k; */ - for (r2 = 0; r2 < 9; r2++) { - maxval = 0.0F; - for (i = 0; i < 12; i++) { - maxval += (float)b_a[r2 + 9 * i] * x_apr[i]; - } - - b_O[r2] = z[r2] - maxval; - } - - for (r2 = 0; r2 < 12; r2++) { - maxval = 0.0F; - for (i = 0; i < 9; i++) { - maxval += K_k[r2 + 12 * i] * b_O[i]; - } - - x_apo[r2] = x_apr[r2] + maxval; - } - - /* 'AttitudeEKF:195' P_apo=(eye(12)-K_k*H_k)*P_apr; */ - memset(&I[0], 0, 144U * sizeof(signed char)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1; - } - - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 9; r1++) { - maxval += K_k[r2 + 12 * r1] * (float)b_a[r1 + 9 * i]; - } - - A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; - } - } - - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - P_apo[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; - } - } - } - } else { - /* 'AttitudeEKF:196' else */ - /* 'AttitudeEKF:197' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */ - if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) { - /* 'AttitudeEKF:199' R=[r_gyro,0,0; */ - /* 'AttitudeEKF:200' 0,r_gyro,0; */ - /* 'AttitudeEKF:201' 0,0,r_gyro]; */ - /* 'AttitudeEKF:202' R_v=[r_gyro,r_gyro,r_gyro]; */ - /* observation matrix */ - /* 'AttitudeEKF:205' H_k=[ E, Z, Z, Z]; */ - /* 'AttitudeEKF:207' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */ - /* S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'AttitudeEKF:210' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; */ - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 12; i++) { - b_S_k[r2 + 3 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - b_S_k[r2 + 3 * i] += (float)c_a[r2 + 3 * r1] * P_apr[r1 + 12 * i]; - } - } - - for (i = 0; i < 3; i++) { - O[r2 + 3 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - O[r2 + 3 * i] += b_S_k[r2 + 3 * r1] * (float)b_b[r1 + 12 * i]; - } - } - } - - /* 'AttitudeEKF:211' S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; */ - c_r_gyro[0] = r_gyro; - c_r_gyro[1] = r_gyro; - c_r_gyro[2] = r_gyro; - for (r2 = 0; r2 < 3; r2++) { - b_muk[r2] = O[r2 << 2] + c_r_gyro[r2]; - } - - for (r2 = 0; r2 < 3; r2++) { - O[r2 << 2] = b_muk[r2]; - } - - /* 'AttitudeEKF:212' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */ - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 3; i++) { - b_O[i + 3 * r2] = O[r2 + 3 * i]; - } - - for (i = 0; i < 12; i++) { - B[r2 + 3 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - B[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2]; - } - } - } - - r1 = 0; - r2 = 1; - r3 = 2; - maxval = (real32_T)fabs(b_O[0]); - a21 = (real32_T)fabs(b_O[1]); - if (a21 > maxval) { - maxval = a21; - r1 = 1; - r2 = 0; - } - - if ((real32_T)fabs(b_O[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } - - b_O[r2] /= b_O[r1]; - b_O[r3] /= b_O[r1]; - b_O[3 + r2] -= b_O[r2] * b_O[3 + r1]; - b_O[3 + r3] -= b_O[r3] * b_O[3 + r1]; - b_O[6 + r2] -= b_O[r2] * b_O[6 + r1]; - b_O[6 + r3] -= b_O[r3] * b_O[6 + r1]; - if ((real32_T)fabs(b_O[3 + r3]) > (real32_T)fabs(b_O[3 + r2])) { - i = r2; - r2 = r3; - r3 = i; - } - - b_O[3 + r3] /= b_O[3 + r2]; - b_O[6 + r3] -= b_O[3 + r3] * b_O[6 + r2]; - for (i = 0; i < 12; i++) { - Y[3 * i] = B[r1 + 3 * i]; - Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * b_O[r2]; - Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * b_O[r3]) - Y[1 + 3 * i] * - b_O[3 + r3]; - Y[2 + 3 * i] /= b_O[6 + r3]; - Y[3 * i] -= Y[2 + 3 * i] * b_O[6 + r1]; - Y[1 + 3 * i] -= Y[2 + 3 * i] * b_O[6 + r2]; - Y[1 + 3 * i] /= b_O[3 + r2]; - Y[3 * i] -= Y[1 + 3 * i] * b_O[3 + r1]; - Y[3 * i] /= b_O[r1]; - } - - for (r2 = 0; r2 < 3; r2++) { - for (i = 0; i < 12; i++) { - b_S_k[i + 12 * r2] = Y[r2 + 3 * i]; - } - } - - /* 'AttitudeEKF:215' x_apo=x_apr+K_k*y_k; */ - for (r2 = 0; r2 < 3; r2++) { - maxval = 0.0F; - for (i = 0; i < 12; i++) { - maxval += (float)c_a[r2 + 3 * i] * x_apr[i]; - } - - b_muk[r2] = z[r2] - maxval; - } - - for (r2 = 0; r2 < 12; r2++) { - maxval = 0.0F; - for (i = 0; i < 3; i++) { - maxval += b_S_k[r2 + 12 * i] * b_muk[i]; - } - - x_apo[r2] = x_apr[r2] + maxval; - } - - /* 'AttitudeEKF:216' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */ - memset(&I[0], 0, 144U * sizeof(signed char)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1; - } - - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 3; r1++) { - maxval += b_S_k[r2 + 12 * r1] * (float)c_a[r1 + 3 * i]; - } - - A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; - } - } - - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - P_apo[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; - } - } - } - } else { - /* 'AttitudeEKF:217' else */ - /* 'AttitudeEKF:218' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */ - if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) { - /* R=[r_gyro,0,0,0,0,0; */ - /* 0,r_gyro,0,0,0,0; */ - /* 0,0,r_gyro,0,0,0; */ - /* 0,0,0,r_accel,0,0; */ - /* 0,0,0,0,r_accel,0; */ - /* 0,0,0,0,0,r_accel]; */ - /* 'AttitudeEKF:227' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; */ - /* observation matrix */ - /* 'AttitudeEKF:230' H_k=[ E, Z, Z, Z; */ - /* 'AttitudeEKF:231' Z, Z, E, Z]; */ - /* 'AttitudeEKF:233' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */ - /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'AttitudeEKF:236' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ - for (r2 = 0; r2 < 6; r2++) { - for (i = 0; i < 12; i++) { - d_a[r2 + 6 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - d_a[r2 + 6 * i] += (float)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; - } - } - - for (i = 0; i < 6; i++) { - b_S_k[r2 + 6 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)c_b[r1 + 12 * i]; - } - } - } - - /* 'AttitudeEKF:237' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ - d_r_gyro[0] = r_gyro; - d_r_gyro[1] = r_gyro; - d_r_gyro[2] = r_gyro; - d_r_gyro[3] = r_accel; - d_r_gyro[4] = r_accel; - d_r_gyro[5] = r_accel; - for (r2 = 0; r2 < 6; r2++) { - c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; - } - - for (r2 = 0; r2 < 6; r2++) { - b_S_k[7 * r2] = c_S_k[r2]; - } - - /* 'AttitudeEKF:238' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 6; i++) { - d_a[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * i]; - } - } - } - - b_mrdivide(d_a, b_S_k, b_K_k); - - /* 'AttitudeEKF:241' x_apo=x_apr+K_k*y_k; */ - for (r2 = 0; r2 < 6; r2++) { - maxval = 0.0F; - for (i = 0; i < 12; i++) { - maxval += (float)e_a[r2 + 6 * i] * x_apr[i]; - } - - d_r_gyro[r2] = z[r2] - maxval; - } - - for (r2 = 0; r2 < 12; r2++) { - maxval = 0.0F; - for (i = 0; i < 6; i++) { - maxval += b_K_k[r2 + 12 * i] * d_r_gyro[i]; - } - - x_apo[r2] = x_apr[r2] + maxval; - } - - /* 'AttitudeEKF:242' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ - memset(&I[0], 0, 144U * sizeof(signed char)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1; - } - - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 6; r1++) { - maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 6 * i]; - } - - A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; - } - } - - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - P_apo[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; - } - } - } - } else { - /* 'AttitudeEKF:243' else */ - /* 'AttitudeEKF:244' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */ - if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) { - /* R=[r_gyro,0,0,0,0,0; */ - /* 0,r_gyro,0,0,0,0; */ - /* 0,0,r_gyro,0,0,0; */ - /* 0,0,0,r_mag,0,0; */ - /* 0,0,0,0,r_mag,0; */ - /* 0,0,0,0,0,r_mag]; */ - /* 'AttitudeEKF:251' R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; */ - /* observation matrix */ - /* 'AttitudeEKF:254' H_k=[ E, Z, Z, Z; */ - /* 'AttitudeEKF:255' Z, Z, Z, E]; */ - /* 'AttitudeEKF:257' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */ - /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'AttitudeEKF:260' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ - for (r2 = 0; r2 < 6; r2++) { - for (i = 0; i < 12; i++) { - d_a[r2 + 6 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - d_a[r2 + 6 * i] += (float)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; - } - } - - for (i = 0; i < 6; i++) { - b_S_k[r2 + 6 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)d_b[r1 + 12 * i]; - } - } - } - - /* 'AttitudeEKF:261' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ - d_r_gyro[0] = r_gyro; - d_r_gyro[1] = r_gyro; - d_r_gyro[2] = r_gyro; - d_r_gyro[3] = r_mag; - d_r_gyro[4] = r_mag; - d_r_gyro[5] = r_mag; - for (r2 = 0; r2 < 6; r2++) { - c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; - } - - for (r2 = 0; r2 < 6; r2++) { - b_S_k[7 * r2] = c_S_k[r2]; - } - - /* 'AttitudeEKF:262' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 6; i++) { - d_a[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 * i]; - } - } - } - - b_mrdivide(d_a, b_S_k, b_K_k); - - /* 'AttitudeEKF:265' x_apo=x_apr+K_k*y_k; */ - for (r2 = 0; r2 < 3; r2++) { - d_r_gyro[r2] = z[r2]; - } - - for (r2 = 0; r2 < 3; r2++) { - d_r_gyro[r2 + 3] = z[6 + r2]; - } - - for (r2 = 0; r2 < 6; r2++) { - c_S_k[r2] = 0.0F; - for (i = 0; i < 12; i++) { - c_S_k[r2] += (float)f_a[r2 + 6 * i] * x_apr[i]; - } - - b_z[r2] = d_r_gyro[r2] - c_S_k[r2]; - } - - for (r2 = 0; r2 < 12; r2++) { - maxval = 0.0F; - for (i = 0; i < 6; i++) { - maxval += b_K_k[r2 + 12 * i] * b_z[i]; - } - - x_apo[r2] = x_apr[r2] + maxval; - } - - /* 'AttitudeEKF:266' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ - memset(&I[0], 0, 144U * sizeof(signed char)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1; - } - - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 6; r1++) { - maxval += b_K_k[r2 + 12 * r1] * (float)f_a[r1 + 6 * i]; - } - - A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; - } - } - - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 12; i++) { - P_apo[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; - } - } - } - } else { - /* 'AttitudeEKF:267' else */ - /* 'AttitudeEKF:268' x_apo=x_apr; */ - for (i = 0; i < 12; i++) { - x_apo[i] = x_apr[i]; - } - - /* 'AttitudeEKF:269' P_apo=P_apr; */ - memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float)); - } - } - } - } - - /* % euler anglels extraction */ - /* 'AttitudeEKF:278' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */ - maxval = norm(*(float (*)[3])&x_apo[6]); - a21 = norm(*(float (*)[3])&x_apo[9]); - for (i = 0; i < 3; i++) { - /* 'AttitudeEKF:279' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */ - muk[i] = -x_apo[i + 6] / maxval; - zek[i] = x_apo[i + 9] / a21; - } - - /* 'AttitudeEKF:281' y_n_b=cross(z_n_b,m_n_b); */ - wak[0] = muk[1] * zek[2] - muk[2] * zek[1]; - wak[1] = muk[2] * zek[0] - muk[0] * zek[2]; - wak[2] = muk[0] * zek[1] - muk[1] * zek[0]; - - /* 'AttitudeEKF:282' y_n_b=y_n_b./norm(y_n_b); */ - maxval = norm(wak); - for (r2 = 0; r2 < 3; r2++) { - wak[r2] /= maxval; - } - - /* 'AttitudeEKF:284' x_n_b=(cross(y_n_b,z_n_b)); */ - zek[0] = wak[1] * muk[2] - wak[2] * muk[1]; - zek[1] = wak[2] * muk[0] - wak[0] * muk[2]; - zek[2] = wak[0] * muk[1] - wak[1] * muk[0]; - - /* 'AttitudeEKF:285' x_n_b=x_n_b./norm(x_n_b); */ - maxval = norm(zek); - for (r2 = 0; r2 < 3; r2++) { - zek[r2] /= maxval; - } - - /* 'AttitudeEKF:288' xa_apo=x_apo; */ - for (i = 0; i < 12; i++) { - xa_apo[i] = x_apo[i]; - } - - /* 'AttitudeEKF:289' Pa_apo=P_apo; */ - memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float)); - - /* rotation matrix from earth to body system */ - /* 'AttitudeEKF:291' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ - for (r2 = 0; r2 < 3; r2++) { - Rot_matrix[r2] = zek[r2]; - Rot_matrix[3 + r2] = wak[r2]; - Rot_matrix[6 + r2] = muk[r2]; - } - - /* 'AttitudeEKF:294' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'AttitudeEKF:295' theta=-asin(Rot_matrix(1,3)); */ - /* 'AttitudeEKF:296' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'AttitudeEKF:297' eulerAngles=[phi;theta;psi]; */ - eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]); - eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); - eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]); -} - -void AttitudeEKF_initialize(void) -{ - Q_not_empty = FALSE; - Ji_not_empty = FALSE; - AttitudeEKF_init(); -} - -void AttitudeEKF_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (AttitudeEKF.c) */ diff --git a/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.h deleted file mode 100644 index 7094da1da5..0000000000 --- a/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - * AttitudeEKF.h - * - * Code generation for function 'AttitudeEKF' - * - * C source code generated on: Thu Aug 21 11:17:28 2014 - * - */ - -#ifndef __ATTITUDEEKF_H__ -#define __ATTITUDEEKF_H__ -/* Include files */ -#include -#include -#include -#include - -#include "rtwtypes.h" -#include "AttitudeEKF_types.h" - -/* Function Declarations */ -extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]); -extern void AttitudeEKF_initialize(void); -extern void AttitudeEKF_terminate(void); -#endif -/* End of code generation (AttitudeEKF.h) */ diff --git a/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF_types.h deleted file mode 100644 index 3f7522ffa0..0000000000 --- a/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF_types.h +++ /dev/null @@ -1,17 +0,0 @@ -/* - * AttitudeEKF_types.h - * - * Code generation for function 'AttitudeEKF' - * - * C source code generated on: Thu Aug 21 11:17:28 2014 - * - */ - -#ifndef __ATTITUDEEKF_TYPES_H__ -#define __ATTITUDEEKF_TYPES_H__ - -/* Include files */ -#include "rtwtypes.h" - -#endif -/* End of code generation (AttitudeEKF_types.h) */ diff --git a/src/examples/attitude_estimator_ekf/codegen/rtwtypes.h b/src/examples/attitude_estimator_ekf/codegen/rtwtypes.h deleted file mode 100644 index b5a02a7a6d..0000000000 --- a/src/examples/attitude_estimator_ekf/codegen/rtwtypes.h +++ /dev/null @@ -1,160 +0,0 @@ -/* - * rtwtypes.h - * - * Code generation for function 'AttitudeEKF' - * - * C source code generated on: Thu Aug 21 11:17:28 2014 - * - */ - -#ifndef __RTWTYPES_H__ -#define __RTWTYPES_H__ -#ifndef TRUE -# define TRUE (1U) -#endif -#ifndef FALSE -# define FALSE (0U) -#endif -#ifndef __TMWTYPES__ -#define __TMWTYPES__ - -#include - -/*=======================================================================* - * Target hardware information - * Device type: ARM Compatible->ARM Cortex - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 - * native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Undefined - * Shift right on a signed integer as arithmetic shift: on - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ - -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * - * ulong_T, char_T and byte_T. * - *===========================================================================*/ - -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef char_T byte_T; - -/*===========================================================================* - * Complex number type definitions * - *===========================================================================*/ -#define CREAL_T - typedef struct { - real32_T re; - real32_T im; - } creal32_T; - - typedef struct { - real64_T re; - real64_T im; - } creal64_T; - - typedef struct { - real_T re; - real_T im; - } creal_T; - - typedef struct { - int8_T re; - int8_T im; - } cint8_T; - - typedef struct { - uint8_T re; - uint8_T im; - } cuint8_T; - - typedef struct { - int16_T re; - int16_T im; - } cint16_T; - - typedef struct { - uint16_T re; - uint16_T im; - } cuint16_T; - - typedef struct { - int32_T re; - int32_T im; - } cint32_T; - - typedef struct { - uint32_T re; - uint32_T im; - } cuint32_T; - - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ - -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255)) -#define MIN_uint8_T ((uint8_T)(0)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535)) -#define MIN_uint16_T ((uint16_T)(0)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MIN_uint32_T ((uint32_T)(0)) - -/* Logical type definitions */ -#if !defined(__cplusplus) && !defined(__true_false_are_keywords) -# ifndef false -# define false (0U) -# endif -# ifndef true -# define true (1U) -# endif -#endif - -/* - * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation - * for signed integer values. - */ -#if ((SCHAR_MIN + 1) != -SCHAR_MAX) -#error "This code must be compiled using a 2's complement representation for signed integer values" -#endif - -/* - * Maximum length of a MATLAB identifier (function/variable) - * including the null-termination character. Referenced by - * rt_logging.c and rt_matrx.c. - */ -#define TMW_NAME_LENGTH_MAX 64 - -#endif -#endif -/* End of code generation (rtwtypes.h) */