fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 03:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged new EKF version

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-16 11:10:09 +02:00
parent a720bfff5e
commit fef4362e79
41 changed files with 709 additions and 722 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+254 -213
View File
@@ -3,19 +3,19 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 17:51:09 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "rdivide.h"
#include "norm.h"
#include "cross.h"
#include "eye.h"
#include "mrdivide.h"
#include "diag.h"
#include "power.h"
/* Type Definitions */
@@ -32,10 +32,24 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);
static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
{
real32_T y;
int32_T b_u0;
int32_T b_u1;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else if (rtIsInfF(u0) && rtIsInfF(u1)) {
y = (real32_T)atan2f(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F);
if (u0 > 0.0F) {
b_u0 = 1;
} else {
b_u0 = -1;
}
if (u1 > 0.0F) {
b_u1 = 1;
} else {
b_u1 = -1;
}
y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);
} else if (u1 == 0.0F) {
if (u0 > 0.0F) {
y = RT_PIF / 2.0F;
@@ -45,7 +59,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
y = 0.0F;
}
} else {
y = (real32_T)atan2f(u0, u1);
y = (real32_T)atan2(u0, u1);
}
return y;
@@ -60,30 +74,41 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
P_aposteriori[144])
{
real32_T a[12];
int32_T i;
real32_T b_a[12];
real32_T Q[144];
real32_T O[9];
real_T dv0[9];
real32_T c_a[9];
real32_T d_a[9];
real32_T a[9];
int32_T i;
real32_T b_a[9];
real32_T x_n_b[3];
real32_T b_x_aposteriori_k[3];
real32_T m_n_b[3];
real32_T z_n_b[3];
real32_T x_apriori[12];
real32_T y_n_b[3];
int32_T i0;
real32_T e_a[3];
real_T dv1[144];
real32_T A_lin[144];
real32_T b_A_lin[144];
int32_T i1;
real32_T y;
real32_T P_apriori[144];
real32_T f0;
static const real32_T fv0[144] = { 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.1F, 0.0F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.1F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F,
0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
0.1F };
real32_T R[81];
real32_T b_P_apriori[108];
static const int8_T iv0[108] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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, 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 };
@@ -91,46 +116,45 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
real32_T K_k[108];
static const int8_T iv1[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, 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,
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 };
real32_T fv0[81];
real32_T fv1[81];
real32_T c_P_apriori[36];
static const int8_T iv2[36] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 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 };
real32_T fv1[36];
real32_T fv2[36];
static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
real32_T S_k[36];
real32_T d_P_apriori[72];
static const int8_T iv4[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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, 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 };
real32_T b_K_k[72];
static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 1, 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, 1, 0, 0, 0, 0, 0, 0, 1, 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 };
real32_T b_r[6];
static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 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, 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, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 1 };
static const int8_T iv7[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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, 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 };
real32_T fv2[6];
real32_T fv3[6];
real32_T b_z[6];
real32_T b_y;
/* Extended Attitude Kalmanfilter */
/* */
@@ -146,32 +170,37 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
/* coder.varsize('udpIndVect', [9,1], [1,0]) */
/* udpIndVect=find(updVect); */
/* process and measurement noise covariance matrix */
/* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */
power(q, 2.0, a);
for (i = 0; i < 12; i++) {
b_a[i] = a[i] * dt;
}
diag(b_a, Q);
/* Q = diag(q.^2*dt); */
/* 'attitudeKalmanfilter:29' Q=[1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:30' 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:31' 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:32' 0, 0, 0, 1e-10, 0, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */
/* 'attitudeKalmanfilter:33' 0, 0, 0, 0, 1e-10, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */
/* 'attitudeKalmanfilter:34' 0, 0, 0, 0, 0, 1e-10, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:35' 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:36' 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:37' 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0; */
/* 'attitudeKalmanfilter:38' 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0; */
/* 'attitudeKalmanfilter:39' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0; */
/* 'attitudeKalmanfilter:40' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1]; */
/* observation matrix */
/* 'attitudeKalmanfilter:37' wx= x_aposteriori_k(1); */
/* 'attitudeKalmanfilter:38' wy= x_aposteriori_k(2); */
/* 'attitudeKalmanfilter:39' wz= x_aposteriori_k(3); */
/* 'attitudeKalmanfilter:41' wox= x_aposteriori_k(4); */
/* 'attitudeKalmanfilter:42' woy= x_aposteriori_k(5); */
/* 'attitudeKalmanfilter:43' woz= x_aposteriori_k(6); */
/* 'attitudeKalmanfilter:45' zex= x_aposteriori_k(7); */
/* 'attitudeKalmanfilter:46' zey= x_aposteriori_k(8); */
/* 'attitudeKalmanfilter:47' zez= x_aposteriori_k(9); */
/* 'attitudeKalmanfilter:49' mux= x_aposteriori_k(10); */
/* 'attitudeKalmanfilter:50' muy= x_aposteriori_k(11); */
/* 'attitudeKalmanfilter:51' muz= x_aposteriori_k(12); */
/* 'attitudeKalmanfilter:54' wk =[wx; */
/* 'attitudeKalmanfilter:55' wy; */
/* 'attitudeKalmanfilter:56' wz]; */
/* 'attitudeKalmanfilter:58' wok =[wox;woy;woz]; */
/* 'attitudeKalmanfilter:59' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
/* 'attitudeKalmanfilter:44' wx= x_aposteriori_k(1); */
/* 'attitudeKalmanfilter:45' wy= x_aposteriori_k(2); */
/* 'attitudeKalmanfilter:46' wz= x_aposteriori_k(3); */
/* 'attitudeKalmanfilter:48' wox= x_aposteriori_k(4); */
/* 'attitudeKalmanfilter:49' woy= x_aposteriori_k(5); */
/* 'attitudeKalmanfilter:50' woz= x_aposteriori_k(6); */
/* 'attitudeKalmanfilter:52' zex= x_aposteriori_k(7); */
/* 'attitudeKalmanfilter:53' zey= x_aposteriori_k(8); */
/* 'attitudeKalmanfilter:54' zez= x_aposteriori_k(9); */
/* 'attitudeKalmanfilter:56' mux= x_aposteriori_k(10); */
/* 'attitudeKalmanfilter:57' muy= x_aposteriori_k(11); */
/* 'attitudeKalmanfilter:58' muz= x_aposteriori_k(12); */
/* 'attitudeKalmanfilter:61' wk =[wx; */
/* 'attitudeKalmanfilter:62' wy; */
/* 'attitudeKalmanfilter:63' wz]; */
/* 'attitudeKalmanfilter:65' wok =[wox;woy;woz]; */
/* 'attitudeKalmanfilter:66' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
O[0] = 0.0F;
O[1] = -x_aposteriori_k[2];
O[2] = x_aposteriori_k[1];
@@ -182,33 +211,36 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
O[7] = x_aposteriori_k[0];
O[8] = 0.0F;
/* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */
/* 'attitudeKalmanfilter:67' zek =(eye(3)+O*dt)*[zex;zey;zez]; */
eye(dv0);
for (i = 0; i < 9; i++) {
c_a[i] = (real32_T)dv0[i] + O[i] * dt;
a[i] = (real32_T)dv0[i] + O[i] * dt;
}
/* 'attitudeKalmanfilter:61' muk =(eye(3)+O*dt)*[mux;muy;muz]; */
/* 'attitudeKalmanfilter:68' muk =(eye(3)+O*dt)*[mux;muy;muz]; */
eye(dv0);
for (i = 0; i < 9; i++) {
d_a[i] = (real32_T)dv0[i] + O[i] * dt;
b_a[i] = (real32_T)dv0[i] + O[i] * dt;
}
/* 'attitudeKalmanfilter:63' EZ=[0,zez,-zey; */
/* 'attitudeKalmanfilter:64' -zez,0,zex; */
/* 'attitudeKalmanfilter:65' zey,-zex,0]'; */
/* 'attitudeKalmanfilter:66' MA=[0,muz,-muy; */
/* 'attitudeKalmanfilter:67' -muz,0,mux; */
/* 'attitudeKalmanfilter:68' zey,-mux,0]'; */
/* 'attitudeKalmanfilter:72' E=eye(3); */
/* 'attitudeKalmanfilter:73' Z=zeros(3); */
/* 'attitudeKalmanfilter:74' x_apriori=[wk;wok;zek;muk]; */
/* 'attitudeKalmanfilter:70' EZ=[0,zez,-zey; */
/* 'attitudeKalmanfilter:71' -zez,0,zex; */
/* 'attitudeKalmanfilter:72' zey,-zex,0]'; */
/* 'attitudeKalmanfilter:73' MA=[0,muz,-muy; */
/* 'attitudeKalmanfilter:74' -muz,0,mux; */
/* 'attitudeKalmanfilter:75' zey,-mux,0]'; */
/* 'attitudeKalmanfilter:79' E=eye(3); */
/* 'attitudeKalmanfilter:80' Es=[1,0,0; */
/* 'attitudeKalmanfilter:81' 0,1,0; */
/* 'attitudeKalmanfilter:82' 0,0,0]; */
/* 'attitudeKalmanfilter:83' Z=zeros(3); */
/* 'attitudeKalmanfilter:84' x_apriori=[wk;wok;zek;muk]; */
x_n_b[0] = x_aposteriori_k[6];
x_n_b[1] = x_aposteriori_k[7];
x_n_b[2] = x_aposteriori_k[8];
z_n_b[0] = x_aposteriori_k[9];
z_n_b[1] = x_aposteriori_k[10];
z_n_b[2] = x_aposteriori_k[11];
b_x_aposteriori_k[0] = x_aposteriori_k[9];
b_x_aposteriori_k[1] = x_aposteriori_k[10];
b_x_aposteriori_k[2] = x_aposteriori_k[11];
x_apriori[0] = x_aposteriori_k[0];
x_apriori[1] = x_aposteriori_k[1];
x_apriori[2] = x_aposteriori_k[2];
@@ -216,28 +248,28 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
x_apriori[4] = x_aposteriori_k[4];
x_apriori[5] = x_aposteriori_k[5];
for (i = 0; i < 3; i++) {
y_n_b[i] = 0.0F;
m_n_b[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
y_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0];
m_n_b[i] += a[i + 3 * i0] * x_n_b[i0];
}
e_a[i] = 0.0F;
z_n_b[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
e_a[i] += d_a[i + 3 * i0] * z_n_b[i0];
z_n_b[i] += b_a[i + 3 * i0] * b_x_aposteriori_k[i0];
}
x_apriori[i + 6] = y_n_b[i];
x_apriori[i + 6] = m_n_b[i];
}
for (i = 0; i < 3; i++) {
x_apriori[i + 9] = e_a[i];
x_apriori[i + 9] = z_n_b[i];
}
/* 'attitudeKalmanfilter:76' A_lin=[ Z, Z, Z, Z */
/* 'attitudeKalmanfilter:77' Z, Z, Z, Z */
/* 'attitudeKalmanfilter:78' EZ, Z, O, Z */
/* 'attitudeKalmanfilter:79' MA, Z, Z, O]; */
/* 'attitudeKalmanfilter:82' A_lin=eye(12)+A_lin*dt; */
/* 'attitudeKalmanfilter:86' A_lin=[ Z, Z, Z, Z */
/* 'attitudeKalmanfilter:87' Z, Z, Z, Z */
/* 'attitudeKalmanfilter:88' EZ, Z, O, Z */
/* 'attitudeKalmanfilter:89' MA, Z, Z, O]; */
/* 'attitudeKalmanfilter:92' A_lin=eye(12)+A_lin*dt; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 3; i0++) {
@@ -310,7 +342,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
}
}
/* 'attitudeKalmanfilter:88' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
/* 'attitudeKalmanfilter:98' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
A_lin[i + 12 * i0] = 0.0F;
@@ -323,28 +355,28 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
y = 0.0F;
f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
y += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
f0 += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
}
P_apriori[i + 12 * i0] = y + Q[i + 12 * i0];
P_apriori[i + 12 * i0] = f0 + fv0[i + 12 * i0];
}
}
/* %update */
/* 'attitudeKalmanfilter:92' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
/* 'attitudeKalmanfilter:102' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {
/* 'attitudeKalmanfilter:93' R=diag(r); */
b_diag(r, R);
/* 'attitudeKalmanfilter:103' R=diag(r); */
diag(r, R);
/* observation matrix */
/* 'attitudeKalmanfilter:96' H_k=[ E, E, Z, Z; */
/* 'attitudeKalmanfilter:97' Z, Z, E, Z; */
/* 'attitudeKalmanfilter:98' Z, Z, Z, E]; */
/* 'attitudeKalmanfilter:100' y_k=z(1:9)-H_k*x_apriori; */
/* 'attitudeKalmanfilter:102' S_k=H_k*P_apriori*H_k'+R; */
/* 'attitudeKalmanfilter:103' K_k=(P_apriori*H_k'/(S_k)); */
/* 'attitudeKalmanfilter:106' H_k=[ E, Es, Z, Z; */
/* 'attitudeKalmanfilter:107' Z, Z, E, Z; */
/* 'attitudeKalmanfilter:108' Z, Z, Z, E]; */
/* 'attitudeKalmanfilter:110' y_k=z(1:9)-H_k*x_apriori; */
/* 'attitudeKalmanfilter:112' S_k=H_k*P_apriori*H_k'+R; */
/* 'attitudeKalmanfilter:113' K_k=(P_apriori*H_k'/(S_k)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 9; i0++) {
b_P_apriori[i + 12 * i0] = 0.0F;
@@ -366,46 +398,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 9; i++) {
for (i0 = 0; i0 < 9; i0++) {
y = 0.0F;
f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
y += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0];
f0 += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0];
}
fv0[i + 9 * i0] = y + R[i + 9 * i0];
fv1[i + 9 * i0] = f0 + R[i + 9 * i0];
}
}
mrdivide(b_P_apriori, fv0, K_k);
mrdivide(b_P_apriori, fv1, K_k);
/* 'attitudeKalmanfilter:106' x_aposteriori=x_apriori+K_k*y_k; */
/* 'attitudeKalmanfilter:116' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 9; i++) {
y = 0.0F;
f0 = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
y += (real32_T)iv1[i + 9 * i0] * x_apriori[i0];
f0 += (real32_T)iv1[i + 9 * i0] * x_apriori[i0];
}
c_a[i] = z[i] - y;
a[i] = z[i] - f0;
}
for (i = 0; i < 12; i++) {
y = 0.0F;
f0 = 0.0F;
for (i0 = 0; i0 < 9; i0++) {
y += K_k[i + 12 * i0] * c_a[i0];
f0 += K_k[i + 12 * i0] * a[i0];
}
x_aposteriori[i] = x_apriori[i] + y;
x_aposteriori[i] = x_apriori[i] + f0;
}
/* 'attitudeKalmanfilter:107' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
/* 'attitudeKalmanfilter:117' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
y = 0.0F;
f0 = 0.0F;
for (i1 = 0; i1 < 9; i1++) {
y += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0];
f0 += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0];
}
Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -413,22 +445,23 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0];
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12
* i0];
}
}
}
} else {
/* 'attitudeKalmanfilter:108' else */
/* 'attitudeKalmanfilter:109' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
/* 'attitudeKalmanfilter:118' else */
/* 'attitudeKalmanfilter:119' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {
/* 'attitudeKalmanfilter:110' R=diag(r(1:3)); */
c_diag(*(real32_T (*)[3])&r[0], O);
/* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */
b_diag(*(real32_T (*)[3])&r[0], O);
/* observation matrix */
/* 'attitudeKalmanfilter:113' H_k=[ E, E, Z, Z]; */
/* 'attitudeKalmanfilter:115' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:117' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
/* 'attitudeKalmanfilter:118' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */
/* 'attitudeKalmanfilter:123' H_k=[ E, Es, Z, Z]; */
/* 'attitudeKalmanfilter:125' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:127' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
/* 'attitudeKalmanfilter:128' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 3; i0++) {
c_P_apriori[i + 12 * i0] = 0.0F;
@@ -441,9 +474,9 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 12; i0++) {
fv1[i + 3 * i0] = 0.0F;
fv2[i + 3 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 *
fv2[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 *
i0];
}
}
@@ -451,46 +484,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
y = 0.0F;
f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
y += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];
f0 += fv2[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];
}
c_a[i + 3 * i0] = y + O[i + 3 * i0];
a[i + 3 * i0] = f0 + O[i + 3 * i0];
}
}
b_mrdivide(c_P_apriori, c_a, S_k);
b_mrdivide(c_P_apriori, a, S_k);
/* 'attitudeKalmanfilter:121' x_aposteriori=x_apriori+K_k*y_k; */
/* 'attitudeKalmanfilter:131' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 3; i++) {
y = 0.0F;
f0 = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
y += (real32_T)iv3[i + 3 * i0] * x_apriori[i0];
f0 += (real32_T)iv3[i + 3 * i0] * x_apriori[i0];
}
x_n_b[i] = z[i] - y;
x_n_b[i] = z[i] - f0;
}
for (i = 0; i < 12; i++) {
y = 0.0F;
f0 = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
y += S_k[i + 12 * i0] * x_n_b[i0];
f0 += S_k[i + 12 * i0] * x_n_b[i0];
}
x_aposteriori[i] = x_apriori[i] + y;
x_aposteriori[i] = x_apriori[i] + f0;
}
/* 'attitudeKalmanfilter:122' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */
/* 'attitudeKalmanfilter:132' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
y = 0.0F;
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
y += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0];
f0 += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0];
}
Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -498,25 +531,25 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *
i0];
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +
12 * i0];
}
}
}
} else {
/* 'attitudeKalmanfilter:123' else */
/* 'attitudeKalmanfilter:124' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */
/* 'attitudeKalmanfilter:133' else */
/* 'attitudeKalmanfilter:134' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */
if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))
{
/* 'attitudeKalmanfilter:125' R=diag(r(1:6)); */
d_diag(*(real32_T (*)[6])&r[0], S_k);
/* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */
c_diag(*(real32_T (*)[6])&r[0], S_k);
/* observation matrix */
/* 'attitudeKalmanfilter:128' H_k=[ E, E, Z, Z; */
/* 'attitudeKalmanfilter:129' Z, Z, E, Z]; */
/* 'attitudeKalmanfilter:131' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:133' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
/* 'attitudeKalmanfilter:134' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
/* 'attitudeKalmanfilter:138' H_k=[ E, Es, Z, Z; */
/* 'attitudeKalmanfilter:139' Z, Z, E, Z]; */
/* 'attitudeKalmanfilter:141' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:143' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
/* 'attitudeKalmanfilter:144' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 6; i0++) {
d_P_apriori[i + 12 * i0] = 0.0F;
@@ -539,46 +572,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 6; i++) {
for (i0 = 0; i0 < 6; i0++) {
y = 0.0F;
f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
y += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0];
f0 += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0];
}
fv1[i + 6 * i0] = y + S_k[i + 6 * i0];
fv2[i + 6 * i0] = f0 + S_k[i + 6 * i0];
}
}
c_mrdivide(d_P_apriori, fv1, b_K_k);
c_mrdivide(d_P_apriori, fv2, b_K_k);
/* 'attitudeKalmanfilter:137' x_aposteriori=x_apriori+K_k*y_k; */
/* 'attitudeKalmanfilter:147' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 6; i++) {
y = 0.0F;
f0 = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
y += (real32_T)iv5[i + 6 * i0] * x_apriori[i0];
f0 += (real32_T)iv5[i + 6 * i0] * x_apriori[i0];
}
b_r[i] = z[i] - y;
b_r[i] = z[i] - f0;
}
for (i = 0; i < 12; i++) {
y = 0.0F;
f0 = 0.0F;
for (i0 = 0; i0 < 6; i0++) {
y += b_K_k[i + 12 * i0] * b_r[i0];
f0 += b_K_k[i + 12 * i0] * b_r[i0];
}
x_aposteriori[i] = x_apriori[i] + y;
x_aposteriori[i] = x_apriori[i] + f0;
}
/* 'attitudeKalmanfilter:138' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
/* 'attitudeKalmanfilter:148' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
y = 0.0F;
f0 = 0.0F;
for (i1 = 0; i1 < 6; i1++) {
y += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0];
f0 += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0];
}
Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -586,22 +619,22 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *
i0];
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1
+ 12 * i0];
}
}
}
} else {
/* 'attitudeKalmanfilter:139' else */
/* 'attitudeKalmanfilter:140' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */
/* 'attitudeKalmanfilter:149' else */
/* 'attitudeKalmanfilter:150' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */
if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))
{
/* 'attitudeKalmanfilter:141' R=diag([r(1:3);r(7:9)]); */
/* 'attitudeKalmanfilter:151' R=diag([r(1:3);r(7:9)]); */
/* observation matrix */
/* 'attitudeKalmanfilter:144' H_k=[ E, E, Z, Z; */
/* 'attitudeKalmanfilter:145' Z, Z, Z, E]; */
/* 'attitudeKalmanfilter:147' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:149' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
/* 'attitudeKalmanfilter:154' H_k=[ E, Es, Z, Z; */
/* 'attitudeKalmanfilter:155' Z, Z, Z, E]; */
/* 'attitudeKalmanfilter:157' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:159' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
for (i = 0; i < 6; i++) {
for (i0 = 0; i0 < 12; i0++) {
b_K_k[i + 6 * i0] = 0.0F;
@@ -619,16 +652,16 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 6; i++) {
for (i0 = 0; i0 < 6; i0++) {
y = 0.0F;
f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
y += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0];
f0 += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0];
}
S_k[i + 6 * i0] = y + b_r[3 * (i + i0)];
S_k[i + 6 * i0] = f0 + b_r[3 * (i + i0)];
}
}
/* 'attitudeKalmanfilter:150' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
/* 'attitudeKalmanfilter:160' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 6; i0++) {
d_P_apriori[i + 12 * i0] = 0.0F;
@@ -641,7 +674,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
c_mrdivide(d_P_apriori, S_k, b_K_k);
/* 'attitudeKalmanfilter:153' x_aposteriori=x_apriori+K_k*y_k; */
/* 'attitudeKalmanfilter:163' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 3; i++) {
b_r[i] = z[i];
}
@@ -651,33 +684,33 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
}
for (i = 0; i < 6; i++) {
fv2[i] = 0.0F;
fv3[i] = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
fv3[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
}
b_z[i] = b_r[i] - fv2[i];
b_z[i] = b_r[i] - fv3[i];
}
for (i = 0; i < 12; i++) {
y = 0.0F;
f0 = 0.0F;
for (i0 = 0; i0 < 6; i0++) {
y += b_K_k[i + 12 * i0] * b_z[i0];
f0 += b_K_k[i + 12 * i0] * b_z[i0];
}
x_aposteriori[i] = x_apriori[i] + y;
x_aposteriori[i] = x_apriori[i] + f0;
}
/* 'attitudeKalmanfilter:154' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
/* 'attitudeKalmanfilter:164' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
y = 0.0F;
f0 = 0.0F;
for (i1 = 0; i1 < 6; i1++) {
y += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
f0 += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
}
Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -685,66 +718,74 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12
* i0];
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *
P_apriori[i1 + 12 * i0];
}
}
}
} else {
/* 'attitudeKalmanfilter:155' else */
/* 'attitudeKalmanfilter:156' x_aposteriori=x_apriori; */
/* 'attitudeKalmanfilter:165' else */
/* 'attitudeKalmanfilter:166' x_aposteriori=x_apriori; */
for (i = 0; i < 12; i++) {
x_aposteriori[i] = x_apriori[i];
}
/* 'attitudeKalmanfilter:157' P_aposteriori=P_apriori; */
memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 144U * sizeof
(real32_T));
/* 'attitudeKalmanfilter:167' P_aposteriori=P_apriori; */
memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));
}
}
}
}
/* % euler anglels extraction */
/* 'attitudeKalmanfilter:166' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
y = norm(*(real32_T (*)[3])&x_aposteriori[6]);
/* 'attitudeKalmanfilter:167' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */
b_y = norm(*(real32_T (*)[3])&x_aposteriori[9]);
/* 'attitudeKalmanfilter:169' y_n_b=cross(z_n_b,m_n_b); */
/* 'attitudeKalmanfilter:176' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
for (i = 0; i < 3; i++) {
z_n_b[i] = -x_aposteriori[i + 6] / y;
x_n_b[i] = x_aposteriori[i + 9] / b_y;
x_n_b[i] = -x_aposteriori[i + 6];
}
cross(z_n_b, x_n_b, y_n_b);
rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);
/* 'attitudeKalmanfilter:170' y_n_b=y_n_b./norm(y_n_b); */
y = norm(y_n_b);
/* 'attitudeKalmanfilter:177' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */
rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])&
x_aposteriori[9]), m_n_b);
/* 'attitudeKalmanfilter:179' y_n_b=cross(z_n_b,m_n_b); */
for (i = 0; i < 3; i++) {
y_n_b[i] /= y;
x_n_b[i] = m_n_b[i];
}
/* 'attitudeKalmanfilter:172' x_n_b=(cross(y_n_b,z_n_b)); */
cross(y_n_b, z_n_b, x_n_b);
cross(z_n_b, x_n_b, m_n_b);
/* 'attitudeKalmanfilter:173' x_n_b=x_n_b./norm(x_n_b); */
y = norm(x_n_b);
/* 'attitudeKalmanfilter:180' y_n_b=y_n_b./norm(y_n_b); */
for (i = 0; i < 3; i++) {
/* 'attitudeKalmanfilter:179' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
Rot_matrix[i] = x_n_b[i] / y;
Rot_matrix[3 + i] = y_n_b[i];
x_n_b[i] = m_n_b[i];
}
rdivide(x_n_b, norm(m_n_b), m_n_b);
/* 'attitudeKalmanfilter:182' x_n_b=(cross(y_n_b,z_n_b)); */
cross(m_n_b, z_n_b, x_n_b);
/* 'attitudeKalmanfilter:183' x_n_b=x_n_b./norm(x_n_b); */
for (i = 0; i < 3; i++) {
b_x_aposteriori_k[i] = x_n_b[i];
}
rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);
/* 'attitudeKalmanfilter:189' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
for (i = 0; i < 3; i++) {
Rot_matrix[i] = x_n_b[i];
Rot_matrix[3 + i] = m_n_b[i];
Rot_matrix[6 + i] = z_n_b[i];
}
/* 'attitudeKalmanfilter:183' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
/* 'attitudeKalmanfilter:184' theta=-asin(Rot_matrix(1,3)); */
/* 'attitudeKalmanfilter:185' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
/* 'attitudeKalmanfilter:186' eulerAngles=[phi;theta;psi]; */
/* 'attitudeKalmanfilter:193' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
/* 'attitudeKalmanfilter:194' theta=-asin(Rot_matrix(1,3)); */
/* 'attitudeKalmanfilter:195' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
/* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */
eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);
eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
}
/* End of code generation (attitudeKalmanfilter.c) */
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -11,7 +11,7 @@
#define __ATTITUDEKALMANFILTER_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -11,7 +11,7 @@
#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -11,7 +11,7 @@
#define __ATTITUDEKALMANFILTER_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/cross.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'cross'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/cross.h
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'cross'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -11,7 +11,7 @@
#define __CROSS_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/diag.c
+8 -20
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'diag'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -27,19 +27,7 @@
/*
*
*/
void b_diag(const real32_T v[9], real32_T d[81])
{
int32_T j;
memset((void *)&d[0], 0, 81U * sizeof(real32_T));
for (j = 0; j < 9; j++) {
d[j + 9 * j] = v[j];
}
}
/*
*
*/
void c_diag(const real32_T v[3], real32_T d[9])
void b_diag(const real32_T v[3], real32_T d[9])
{
int32_T j;
for (j = 0; j < 9; j++) {
@@ -54,10 +42,10 @@ void c_diag(const real32_T v[3], real32_T d[9])
/*
*
*/
void d_diag(const real32_T v[6], real32_T d[36])
void c_diag(const real32_T v[6], real32_T d[36])
{
int32_T j;
memset((void *)&d[0], 0, 36U * sizeof(real32_T));
memset(&d[0], 0, 36U * sizeof(real32_T));
for (j = 0; j < 6; j++) {
d[j + 6 * j] = v[j];
}
@@ -66,12 +54,12 @@ void d_diag(const real32_T v[6], real32_T d[36])
/*
*
*/
void diag(const real32_T v[12], real32_T d[144])
void diag(const real32_T v[9], real32_T d[81])
{
int32_T j;
memset((void *)&d[0], 0, 144U * sizeof(real32_T));
for (j = 0; j < 12; j++) {
d[j + 12 * j] = v[j];
memset(&d[0], 0, 81U * sizeof(real32_T));
for (j = 0; j < 9; j++) {
d[j + 9 * j] = v[j];
}
}
apps/attitude_estimator_ekf/codegen/diag.h
+5 -6
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'diag'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -11,7 +11,7 @@
#define __DIAG_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
@@ -29,9 +29,8 @@
/* Variable Definitions */
/* Function Declarations */
extern void b_diag(const real32_T v[9], real32_T d[81]);
extern void c_diag(const real32_T v[3], real32_T d[9]);
extern void d_diag(const real32_T v[6], real32_T d[36]);
extern void diag(const real32_T v[12], real32_T d[144]);
extern void b_diag(const real32_T v[3], real32_T d[9]);
extern void c_diag(const real32_T v[6], real32_T d[36]);
extern void diag(const real32_T v[9], real32_T d[81]);
#endif
/* End of code generation (diag.h) */
apps/attitude_estimator_ekf/codegen/eye.c
+3 -3
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -30,7 +30,7 @@
void b_eye(real_T I[144])
{
int32_T i;
memset((void *)&I[0], 0, 144U * sizeof(real_T));
memset(&I[0], 0, 144U * sizeof(real_T));
for (i = 0; i < 12; i++) {
I[i + 12 * i] = 1.0;
}
@@ -42,7 +42,7 @@ void b_eye(real_T I[144])
void eye(real_T I[9])
{
int32_T i;
memset((void *)&I[0], 0, 9U * sizeof(real_T));
memset(&I[0], 0, 9U * sizeof(real_T));
for (i = 0; i < 3; i++) {
I[i + 3 * i] = 1.0;
}
apps/attitude_estimator_ekf/codegen/eye.h
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -11,7 +11,7 @@
#define __EYE_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/mrdivide.c
+104 -114
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -29,9 +29,9 @@
*/
void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
{
real32_T b_A[9];
int32_T rtemp;
int32_T k;
real32_T b_A[9];
real32_T b_B[36];
int32_T r1;
int32_T r2;
@@ -54,15 +54,15 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
r1 = 0;
r2 = 1;
r3 = 2;
maxval = (real32_T)fabsf(b_A[0]);
a21 = (real32_T)fabsf(b_A[1]);
maxval = (real32_T)fabs(b_A[0]);
a21 = (real32_T)fabs(b_A[1]);
if (a21 > maxval) {
maxval = a21;
r1 = 1;
r2 = 0;
}
if ((real32_T)fabsf(b_A[2]) > maxval) {
if ((real32_T)fabs(b_A[2]) > maxval) {
r1 = 2;
r2 = 1;
r3 = 0;
@@ -74,7 +74,7 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
if ((real32_T)fabsf(b_A[3 + r3]) > (real32_T)fabsf(b_A[3 + r2])) {
if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
rtemp = r2;
r2 = r3;
r3 = rtemp;
@@ -107,51 +107,46 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
*/
void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
{
int32_T jy;
int32_T iy;
real32_T b_A[36];
int8_T ipiv[6];
int32_T i3;
int32_T iy;
int32_T j;
int32_T mmj;
int32_T jj;
int32_T jp1j;
int32_T c;
int32_T ix;
real32_T temp;
int32_T k;
real32_T s;
int32_T loop_ub;
int32_T jy;
int32_T ijA;
real32_T Y[72];
for (jy = 0; jy < 6; jy++) {
for (i3 = 0; i3 < 6; i3++) {
for (iy = 0; iy < 6; iy++) {
b_A[iy + 6 * jy] = B[jy + 6 * iy];
b_A[iy + 6 * i3] = B[i3 + 6 * iy];
}
ipiv[jy] = (int8_T)(1 + jy);
ipiv[i3] = (int8_T)(1 + i3);
}
for (j = 0; j < 5; j++) {
mmj = -j;
jj = j * 7;
jp1j = jj + 1;
c = mmj + 6;
jy = 0;
ix = jj;
temp = (real32_T)fabsf(b_A[jj]);
for (k = 2; k <= c; k++) {
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)fabsf(b_A[ix]);
s = (real32_T)fabs(b_A[ix]);
if (s > temp) {
jy = k - 1;
iy = k - 1;
temp = s;
}
}
if ((real_T)b_A[jj + jy] != 0.0) {
if (jy != 0) {
ipiv[j] = (int8_T)((j + jy) + 1);
if (b_A[c + iy] != 0.0F) {
if (iy != 0) {
ipiv[j] = (int8_T)((j + iy) + 1);
ix = j;
iy = j + jy;
iy += j;
for (k = 0; k < 6; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
@@ -161,42 +156,42 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
}
}
loop_ub = (jp1j + mmj) + 5;
for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
b_A[iy] /= b_A[jj];
i3 = (c - j) + 6;
for (jy = c + 1; jy + 1 <= i3; jy++) {
b_A[jy] /= b_A[c];
}
}
c = 5 - j;
jy = jj + 6;
for (iy = 1; iy <= c; iy++) {
if ((real_T)b_A[jy] != 0.0) {
temp = b_A[jy] * -1.0F;
ix = jp1j;
loop_ub = (mmj + jj) + 12;
for (k = 7 + jj; k + 1 <= loop_ub; k++) {
b_A[k] += b_A[ix] * temp;
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;
i3 = (iy - j) + 12;
for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
b_A[ijA] += b_A[ix] * -temp;
ix++;
}
}
jy += 6;
jj += 6;
iy += 6;
}
}
for (jy = 0; jy < 12; jy++) {
for (i3 = 0; i3 < 12; i3++) {
for (iy = 0; iy < 6; iy++) {
Y[iy + 6 * jy] = A[jy + 12 * iy];
Y[iy + 6 * i3] = A[i3 + 12 * iy];
}
}
for (iy = 0; iy < 6; iy++) {
if (ipiv[iy] != iy + 1) {
for (jy = 0; jy < 6; jy++) {
if (ipiv[jy] != jy + 1) {
for (j = 0; j < 12; j++) {
temp = Y[iy + 6 * j];
Y[iy + 6 * j] = Y[(ipiv[iy] + 6 * j) - 1];
Y[(ipiv[iy] + 6 * j) - 1] = temp;
temp = Y[jy + 6 * j];
Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
Y[(ipiv[jy] + 6 * j) - 1] = temp;
}
}
}
@@ -204,10 +199,10 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
for (j = 0; j < 12; j++) {
c = 6 * j;
for (k = 0; k < 6; k++) {
jy = 6 * k;
if ((real_T)Y[k + c] != 0.0) {
for (iy = k + 2; iy < 7; iy++) {
Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
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];
}
}
}
@@ -216,19 +211,19 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
for (j = 0; j < 12; j++) {
c = 6 * j;
for (k = 5; k > -1; k += -1) {
jy = 6 * k;
if ((real_T)Y[k + c] != 0.0) {
Y[k + c] /= b_A[k + jy];
for (iy = 0; iy + 1 <= k; iy++) {
Y[iy + c] -= Y[k + c] * b_A[iy + jy];
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 (jy = 0; jy < 6; jy++) {
for (i3 = 0; i3 < 6; i3++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * jy] = Y[jy + 6 * iy];
y[iy + 12 * i3] = Y[i3 + 6 * iy];
}
}
}
@@ -238,51 +233,46 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
*/
void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
{
int32_T jy;
int32_T iy;
real32_T b_A[81];
int8_T ipiv[9];
int32_T i2;
int32_T iy;
int32_T j;
int32_T mmj;
int32_T jj;
int32_T jp1j;
int32_T c;
int32_T ix;
real32_T temp;
int32_T k;
real32_T s;
int32_T loop_ub;
int32_T jy;
int32_T ijA;
real32_T Y[108];
for (jy = 0; jy < 9; jy++) {
for (i2 = 0; i2 < 9; i2++) {
for (iy = 0; iy < 9; iy++) {
b_A[iy + 9 * jy] = B[jy + 9 * iy];
b_A[iy + 9 * i2] = B[i2 + 9 * iy];
}
ipiv[jy] = (int8_T)(1 + jy);
ipiv[i2] = (int8_T)(1 + i2);
}
for (j = 0; j < 8; j++) {
mmj = -j;
jj = j * 10;
jp1j = jj + 1;
c = mmj + 9;
jy = 0;
ix = jj;
temp = (real32_T)fabsf(b_A[jj]);
for (k = 2; k <= c; k++) {
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)fabsf(b_A[ix]);
s = (real32_T)fabs(b_A[ix]);
if (s > temp) {
jy = k - 1;
iy = k - 1;
temp = s;
}
}
if ((real_T)b_A[jj + jy] != 0.0) {
if (jy != 0) {
ipiv[j] = (int8_T)((j + jy) + 1);
if (b_A[c + iy] != 0.0F) {
if (iy != 0) {
ipiv[j] = (int8_T)((j + iy) + 1);
ix = j;
iy = j + jy;
iy += j;
for (k = 0; k < 9; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
@@ -292,42 +282,42 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
}
}
loop_ub = (jp1j + mmj) + 8;
for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
b_A[iy] /= b_A[jj];
i2 = (c - j) + 9;
for (jy = c + 1; jy + 1 <= i2; jy++) {
b_A[jy] /= b_A[c];
}
}
c = 8 - j;
jy = jj + 9;
for (iy = 1; iy <= c; iy++) {
if ((real_T)b_A[jy] != 0.0) {
temp = b_A[jy] * -1.0F;
ix = jp1j;
loop_ub = (mmj + jj) + 18;
for (k = 10 + jj; k + 1 <= loop_ub; k++) {
b_A[k] += b_A[ix] * temp;
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;
i2 = (iy - j) + 18;
for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
b_A[ijA] += b_A[ix] * -temp;
ix++;
}
}
jy += 9;
jj += 9;
iy += 9;
}
}
for (jy = 0; jy < 12; jy++) {
for (i2 = 0; i2 < 12; i2++) {
for (iy = 0; iy < 9; iy++) {
Y[iy + 9 * jy] = A[jy + 12 * iy];
Y[iy + 9 * i2] = A[i2 + 12 * iy];
}
}
for (iy = 0; iy < 9; iy++) {
if (ipiv[iy] != iy + 1) {
for (jy = 0; jy < 9; jy++) {
if (ipiv[jy] != jy + 1) {
for (j = 0; j < 12; j++) {
temp = Y[iy + 9 * j];
Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1];
Y[(ipiv[iy] + 9 * j) - 1] = temp;
temp = Y[jy + 9 * j];
Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
Y[(ipiv[jy] + 9 * j) - 1] = temp;
}
}
}
@@ -335,10 +325,10 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
for (j = 0; j < 12; j++) {
c = 9 * j;
for (k = 0; k < 9; k++) {
jy = 9 * k;
if ((real_T)Y[k + c] != 0.0) {
for (iy = k + 2; iy < 10; iy++) {
Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
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];
}
}
}
@@ -347,19 +337,19 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
for (j = 0; j < 12; j++) {
c = 9 * j;
for (k = 8; k > -1; k += -1) {
jy = 9 * k;
if ((real_T)Y[k + c] != 0.0) {
Y[k + c] /= b_A[k + jy];
for (iy = 0; iy + 1 <= k; iy++) {
Y[iy + c] -= Y[k + c] * b_A[iy + jy];
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 (jy = 0; jy < 9; jy++) {
for (i2 = 0; i2 < 9; i2++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * jy] = Y[jy + 9 * iy];
y[iy + 12 * i2] = Y[i2 + 9 * iy];
}
}
}
apps/attitude_estimator_ekf/codegen/mrdivide.h
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -11,7 +11,7 @@
#define __MRDIVIDE_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/norm.c
+11 -19
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -31,32 +31,24 @@ real32_T norm(const real32_T x[3])
{
real32_T y;
real32_T scale;
boolean_T firstNonZero;
int32_T k;
real32_T absxk;
real32_T t;
y = 0.0F;
scale = 0.0F;
firstNonZero = TRUE;
scale = 1.17549435E-38F;
for (k = 0; k < 3; k++) {
if (x[k] != 0.0F) {
absxk = (real32_T)fabsf(x[k]);
if (firstNonZero) {
scale = absxk;
y = 1.0F;
firstNonZero = FALSE;
} else if (scale < absxk) {
t = scale / absxk;
y = 1.0F + y * t * t;
scale = absxk;
} else {
t = absxk / scale;
y += t * t;
}
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)sqrtf(y);
return scale * (real32_T)sqrt(y);
}
/* End of code generation (norm.c) */
apps/attitude_estimator_ekf/codegen/norm.h
+2 -2
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -11,7 +11,7 @@
#define __NORM_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
apps/attitude_estimator_ekf/codegen/power.c
-84
View File
@@ -1,84 +0,0 @@
/*
* power.c
*
* Code generation for function 'power'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "power.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
static real32_T rt_powf_snf(real32_T u0, real32_T u1);
/* Function Definitions */
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
{
real32_T y;
real32_T f0;
real32_T f1;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f0 = (real32_T)fabsf(u0);
f1 = (real32_T)fabsf(u1);
if (rtIsInfF(u1)) {
if (f0 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f0 > 1.0F) {
if (u1 > 0.0F) {
y = ((real32_T)rtInf);
} else {
y = 0.0F;
}
} else if (u1 > 0.0F) {
y = 0.0F;
} else {
y = ((real32_T)rtInf);
}
} else if (f1 == 0.0F) {
y = 1.0F;
} else if (f1 == 1.0F) {
if (u1 > 0.0F) {
y = u0;
} else {
y = 1.0F / u0;
}
} else if (u1 == 2.0F) {
y = u0 * u0;
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
y = (real32_T)sqrtf(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floorf(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)powf(u0, u1);
}
}
return y;
}
/*
*
*/
void power(const real32_T a[12], real_T b, real32_T y[12])
{
int32_T k;
for (k = 0; k < 12; k++) {
y[k] = rt_powf_snf(a[k], (real32_T)b);
}
}
/* End of code generation (power.c) */
apps/attitude_estimator_ekf/codegen/rdivide.c
Executable
+38
View File
@@ -0,0 +1,38 @@
/*
* rdivide.c
*
* Code generation for function 'rdivide'
*
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "rdivide.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void rdivide(const real32_T x[3], real32_T y, real32_T z[3])
{
int32_T i;
for (i = 0; i < 3; i++) {
z[i] = x[i] / y;
}
}
/* End of code generation (rdivide.c) */
apps/attitude_estimator_ekf/codegen/power.h → apps/attitude_estimator_ekf/codegen/rdivide.h
+8 -8
View File
@@ -1,17 +1,17 @@
/*
* power.h
* rdivide.h
*
* Code generation for function 'power'
* Code generation for function 'rdivide'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
#ifndef __POWER_H__
#define __POWER_H__
#ifndef __RDIVIDE_H__
#define __RDIVIDE_H__
/* Include files */
#include <math.h>
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
@@ -29,6 +29,6 @@
/* Variable Definitions */
/* Function Declarations */
extern void power(const real32_T a[12], real_T b, real32_T y[12]);
extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);
#endif
/* End of code generation (power.h) */
/* End of code generation (rdivide.h) */
apps/attitude_estimator_ekf/codegen/rtGetInf.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetInf.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_defines.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
+1 -1
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
apps/attitude_estimator_ekf/codegen/rtwtypes.h
+3 -3
View File
@@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
* C source code generated on: Sat Oct 13 16:28:18 2012
*
*/
@@ -26,8 +26,8 @@
* Number of bits: char: 8 short: 16 int: 32
* long: 32 native word size: 32
* Byte ordering: LittleEndian
* Signed integer division rounds to: Zero
* Shift right on a signed integer as arithmetic shift: on
* Signed integer division rounds to: Undefined
* Shift right on a signed integer as arithmetic shift: off
*=======================================================================*/
/*=======================================================================*