fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 15:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Butchered position estimator from Damian Aregger into shape, publishes now global position estimate as well. Compiling, needs HIL testing

Browse Source
This commit is contained in:
Lorenz Meier
2013-03-23 22:39:54 +01:00
parent 0dc96dbd89
commit 482cada59b
56 changed files with 3475 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/position_estimator_mc/Makefile
+64
View File
@@ -0,0 +1,64 @@
############################################################################
#
# Copyright (C) 2012 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.
#
############################################################################
#
# Makefile to build the position estimator
#
APPNAME = position_estimator_mc
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 4096
CSRCS = position_estimator_mc_main.c \
position_estimator_mc_params.c \
codegen/positionKalmanFilter1D_initialize.c \
codegen/positionKalmanFilter1D_terminate.c \
codegen/positionKalmanFilter1D.c \
codegen/rt_nonfinite.c \
codegen/rtGetInf.c \
codegen/rtGetNaN.c \
codegen/positionKalmanFilter1D_dT_initialize.c \
codegen/positionKalmanFilter1D_dT_terminate.c \
codegen/kalman_dlqe1.c \
codegen/kalman_dlqe1_initialize.c \
codegen/kalman_dlqe1_terminate.c \
codegen/kalman_dlqe2.c \
codegen/kalman_dlqe2_initialize.c \
codegen/kalman_dlqe2_terminate.c \
codegen/kalman_dlqe3.c \
codegen/kalman_dlqe3_initialize.c \
codegen/kalman_dlqe3_terminate.c \
codegen/kalman_dlqe3_data.c \
codegen/randn.c
include $(APPDIR)/mk/app.mk
apps/position_estimator_mc/codegen/kalman_dlqe1.c
Executable
+58
View File
@@ -0,0 +1,58 @@
/*
* kalman_dlqe1.c
*
* Code generation for function 'kalman_dlqe1'
*
* C source code generated on: Wed Feb 13 20:34:32 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe1.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3],
const real32_T x_aposteriori_k[3], real32_T z, real32_T
x_aposteriori[3])
{
printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z);
printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2]));
real32_T y;
int32_T i0;
real32_T b_y[3];
int32_T i1;
real32_T f0;
y = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
b_y[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_y[i0] += C[i1] * A[i1 + 3 * i0];
}
y += b_y[i0] * x_aposteriori_k[i0];
}
y = z - y;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
x_aposteriori[i0] = f0 + K[i0] * y;
}
//printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2]));
}
/* End of code generation (kalman_dlqe1.c) */
apps/position_estimator_mc/codegen/kalman_dlqe1.h
Executable
+30
View File
@@ -0,0 +1,30 @@
/*
* kalman_dlqe1.h
*
* Code generation for function 'kalman_dlqe1'
*
* C source code generated on: Wed Feb 13 20:34:32 2013
*
*/
#ifndef __KALMAN_DLQE1_H__
#define __KALMAN_DLQE1_H__
/* Include files */
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "kalman_dlqe1_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
#endif
/* End of code generation (kalman_dlqe1.h) */
apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* kalman_dlqe1_initialize.c
*
* Code generation for function 'kalman_dlqe1_initialize'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe1.h"
#include "kalman_dlqe1_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe1_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (kalman_dlqe1_initialize.c) */
apps/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
Executable
+30
View File
@@ -0,0 +1,30 @@
/*
* kalman_dlqe1_initialize.h
*
* Code generation for function 'kalman_dlqe1_initialize'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
#ifndef __KALMAN_DLQE1_INITIALIZE_H__
#define __KALMAN_DLQE1_INITIALIZE_H__
/* Include files */
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "kalman_dlqe1_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe1_initialize(void);
#endif
/* End of code generation (kalman_dlqe1_initialize.h) */
apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* kalman_dlqe1_terminate.c
*
* Code generation for function 'kalman_dlqe1_terminate'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe1.h"
#include "kalman_dlqe1_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe1_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (kalman_dlqe1_terminate.c) */
apps/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
Executable
+30
View File
@@ -0,0 +1,30 @@
/*
* kalman_dlqe1_terminate.h
*
* Code generation for function 'kalman_dlqe1_terminate'
*
* C source code generated on: Wed Feb 13 20:34:32 2013
*
*/
#ifndef __KALMAN_DLQE1_TERMINATE_H__
#define __KALMAN_DLQE1_TERMINATE_H__
/* Include files */
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "kalman_dlqe1_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe1_terminate(void);
#endif
/* End of code generation (kalman_dlqe1_terminate.h) */
apps/position_estimator_mc/codegen/kalman_dlqe1_types.h
Executable
+16
View File
@@ -0,0 +1,16 @@
/*
* kalman_dlqe1_types.h
*
* Code generation for function 'kalman_dlqe1'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
#ifndef __KALMAN_DLQE1_TYPES_H__
#define __KALMAN_DLQE1_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (kalman_dlqe1_types.h) */
apps/position_estimator_mc/codegen/kalman_dlqe2.c
Executable
+119
View File
@@ -0,0 +1,119 @@
/*
* kalman_dlqe2.c
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe2.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 f1;
real32_T f2;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f1 = (real32_T)fabs(u0);
f2 = (real32_T)fabs(u1);
if (rtIsInfF(u1)) {
if (f1 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f1 > 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 (f2 == 0.0F) {
y = 1.0F;
} else if (f2 == 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)sqrt(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)pow(u0, u1);
}
}
return y;
}
void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
real32_T x_aposteriori_k[3], real32_T z, real32_T
x_aposteriori[3])
{
//printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3));
//printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3));
real32_T A[9];
real32_T y;
int32_T i0;
static const int8_T iv0[3] = { 0, 0, 1 };
real32_T b_k1[3];
int32_T i1;
static const int8_T iv1[3] = { 1, 0, 0 };
real32_T f0;
A[0] = 1.0F;
A[3] = dt;
A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
A[1] = 0.0F;
A[4] = 1.0F;
A[7] = dt;
y = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
A[2 + 3 * i0] = (real32_T)iv0[i0];
b_k1[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
}
y += b_k1[i0] * x_aposteriori_k[i0];
}
y = z - y;
b_k1[0] = k1;
b_k1[1] = k2;
b_k1[2] = k3;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
x_aposteriori[i0] = f0 + b_k1[i0] * y;
}
}
/* End of code generation (kalman_dlqe2.c) */
apps/position_estimator_mc/codegen/kalman_dlqe2.h
Executable
+32
View File
@@ -0,0 +1,32 @@
/*
* kalman_dlqe2.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
#ifndef __KALMAN_DLQE2_H__
#define __KALMAN_DLQE2_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe2_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
#endif
/* End of code generation (kalman_dlqe2.h) */
apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* kalman_dlqe2_initialize.c
*
* Code generation for function 'kalman_dlqe2_initialize'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe2.h"
#include "kalman_dlqe2_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe2_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (kalman_dlqe2_initialize.c) */
apps/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
Executable
+32
View File
@@ -0,0 +1,32 @@
/*
* kalman_dlqe2_initialize.h
*
* Code generation for function 'kalman_dlqe2_initialize'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
#ifndef __KALMAN_DLQE2_INITIALIZE_H__
#define __KALMAN_DLQE2_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe2_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe2_initialize(void);
#endif
/* End of code generation (kalman_dlqe2_initialize.h) */
apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* kalman_dlqe2_terminate.c
*
* Code generation for function 'kalman_dlqe2_terminate'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe2.h"
#include "kalman_dlqe2_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe2_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (kalman_dlqe2_terminate.c) */
apps/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
Executable
+32
View File
@@ -0,0 +1,32 @@
/*
* kalman_dlqe2_terminate.h
*
* Code generation for function 'kalman_dlqe2_terminate'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
#ifndef __KALMAN_DLQE2_TERMINATE_H__
#define __KALMAN_DLQE2_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe2_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe2_terminate(void);
#endif
/* End of code generation (kalman_dlqe2_terminate.h) */
apps/position_estimator_mc/codegen/kalman_dlqe2_types.h
Executable
+16
View File
@@ -0,0 +1,16 @@
/*
* kalman_dlqe2_types.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
#ifndef __KALMAN_DLQE2_TYPES_H__
#define __KALMAN_DLQE2_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (kalman_dlqe2_types.h) */
apps/position_estimator_mc/codegen/kalman_dlqe3.c
Executable
+137
View File
@@ -0,0 +1,137 @@
/*
* kalman_dlqe3.c
*
* Code generation for function 'kalman_dlqe3'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "randn.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 f1;
real32_T f2;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f1 = (real32_T)fabs(u0);
f2 = (real32_T)fabs(u1);
if (rtIsInfF(u1)) {
if (f1 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f1 > 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 (f2 == 0.0F) {
y = 1.0F;
} else if (f2 == 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)sqrt(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)pow(u0, u1);
}
}
return y;
}
void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate,
real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3])
{
real32_T A[9];
int32_T i0;
static const int8_T iv0[3] = { 0, 0, 1 };
real_T b;
real32_T y;
real32_T b_y[3];
int32_T i1;
static const int8_T iv1[3] = { 1, 0, 0 };
real32_T b_k1[3];
real32_T f0;
A[0] = 1.0F;
A[3] = dt;
A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
A[1] = 0.0F;
A[4] = 1.0F;
A[7] = dt;
for (i0 = 0; i0 < 3; i0++) {
A[2 + 3 * i0] = (real32_T)iv0[i0];
}
if (addNoise == 1.0F) {
b = randn();
z += sigma * (real32_T)b;
}
if (posUpdate != 0.0F) {
y = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
b_y[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
}
y += b_y[i0] * x_aposteriori_k[i0];
}
y = z - y;
b_k1[0] = k1;
b_k1[1] = k2;
b_k1[2] = k3;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
x_aposteriori[i0] = f0 + b_k1[i0] * y;
}
} else {
for (i0 = 0; i0 < 3; i0++) {
x_aposteriori[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
}
}
}
/* End of code generation (kalman_dlqe3.c) */
apps/position_estimator_mc/codegen/kalman_dlqe3.h
Executable
+33
View File
@@ -0,0 +1,33 @@
/*
* kalman_dlqe3.h
*
* Code generation for function 'kalman_dlqe3'
*
* C source code generated on: Tue Feb 19 15:26:32 2013
*
*/
#ifndef __KALMAN_DLQE3_H__
#define __KALMAN_DLQE3_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]);
#endif
/* End of code generation (kalman_dlqe3.h) */
apps/position_estimator_mc/codegen/kalman_dlqe3_data.c
Executable
+32
View File
@@ -0,0 +1,32 @@
/*
* kalman_dlqe3_data.c
*
* Code generation for function 'kalman_dlqe3_data'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "kalman_dlqe3_data.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
uint32_T method;
uint32_T state[2];
uint32_T b_method;
uint32_T b_state;
uint32_T c_state[2];
boolean_T state_not_empty;
/* Function Declarations */
/* Function Definitions */
/* End of code generation (kalman_dlqe3_data.c) */
apps/position_estimator_mc/codegen/kalman_dlqe3_data.h
Executable
+38
View File
@@ -0,0 +1,38 @@
/*
* kalman_dlqe3_data.h
*
* Code generation for function 'kalman_dlqe3_data'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
#ifndef __KALMAN_DLQE3_DATA_H__
#define __KALMAN_DLQE3_DATA_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
extern uint32_T method;
extern uint32_T state[2];
extern uint32_T b_method;
extern uint32_T b_state;
extern uint32_T c_state[2];
extern boolean_T state_not_empty;
/* Variable Definitions */
/* Function Declarations */
#endif
/* End of code generation (kalman_dlqe3_data.h) */
apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
Executable
+47
View File
@@ -0,0 +1,47 @@
/*
* kalman_dlqe3_initialize.c
*
* Code generation for function 'kalman_dlqe3_initialize'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "kalman_dlqe3_initialize.h"
#include "kalman_dlqe3_data.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe3_initialize(void)
{
int32_T i;
static const uint32_T uv0[2] = { 362436069U, 0U };
rt_InitInfAndNaN(8U);
state_not_empty = FALSE;
b_state = 1144108930U;
b_method = 7U;
method = 0U;
for (i = 0; i < 2; i++) {
c_state[i] = 362436069U + 158852560U * (uint32_T)i;
state[i] = uv0[i];
}
if (state[1] == 0U) {
state[1] = 521288629U;
}
}
/* End of code generation (kalman_dlqe3_initialize.c) */
apps/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
Executable
+33
View File
@@ -0,0 +1,33 @@
/*
* kalman_dlqe3_initialize.h
*
* Code generation for function 'kalman_dlqe3_initialize'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
#ifndef __KALMAN_DLQE3_INITIALIZE_H__
#define __KALMAN_DLQE3_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe3_initialize(void);
#endif
/* End of code generation (kalman_dlqe3_initialize.h) */
apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* kalman_dlqe3_terminate.c
*
* Code generation for function 'kalman_dlqe3_terminate'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "kalman_dlqe3_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe3_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (kalman_dlqe3_terminate.c) */
apps/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
Executable
+33
View File
@@ -0,0 +1,33 @@
/*
* kalman_dlqe3_terminate.h
*
* Code generation for function 'kalman_dlqe3_terminate'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
#ifndef __KALMAN_DLQE3_TERMINATE_H__
#define __KALMAN_DLQE3_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe3_terminate(void);
#endif
/* End of code generation (kalman_dlqe3_terminate.h) */
apps/position_estimator_mc/codegen/kalman_dlqe3_types.h
Executable
+16
View File
@@ -0,0 +1,16 @@
/*
* kalman_dlqe3_types.h
*
* Code generation for function 'kalman_dlqe3'
*
* C source code generated on: Tue Feb 19 15:26:30 2013
*
*/
#ifndef __KALMAN_DLQE3_TYPES_H__
#define __KALMAN_DLQE3_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (kalman_dlqe3_types.h) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D.c
Executable
+136
View File
@@ -0,0 +1,136 @@
/*
* positionKalmanFilter1D.c
*
* Code generation for function 'positionKalmanFilter1D'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const
real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T
P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T
Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3],
real32_T P_aposteriori[9])
{
int32_T i0;
real32_T f0;
int32_T k;
real32_T b_A[9];
int32_T i1;
real32_T P_apriori[9];
real32_T y;
real32_T K[3];
real32_T S;
int8_T I[9];
/* prediction */
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (k = 0; k < 3; k++) {
f0 += A[i0 + 3 * k] * x_aposteriori_k[k];
}
x_aposteriori[i0] = f0 + B[i0] * u;
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
b_A[i0 + 3 * k] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k];
}
}
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1];
}
P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k];
}
}
if ((real32_T)fabs(u) < thresh) {
x_aposteriori[1] *= decay;
}
/* update */
if (gps_update == 1) {
y = 0.0F;
for (k = 0; k < 3; k++) {
y += C[k] * x_aposteriori[k];
K[k] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
K[k] += C[i0] * P_apriori[i0 + 3 * k];
}
}
y = z - y;
S = 0.0F;
for (k = 0; k < 3; k++) {
S += K[k] * C[k];
}
S += R;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (k = 0; k < 3; k++) {
f0 += P_apriori[i0 + 3 * k] * C[k];
}
K[i0] = f0 / S;
}
for (i0 = 0; i0 < 3; i0++) {
x_aposteriori[i0] += K[i0] * y;
}
for (i0 = 0; i0 < 9; i0++) {
I[i0] = 0;
}
for (k = 0; k < 3; k++) {
I[k + 3 * k] = 1;
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0];
}
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
P_aposteriori[i0 + 3 * k] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k];
}
}
}
} else {
for (i0 = 0; i0 < 9; i0++) {
P_aposteriori[i0] = P_apriori[i0];
}
}
}
/* End of code generation (positionKalmanFilter1D.c) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D.h
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* positionKalmanFilter1D.h
*
* Code generation for function 'positionKalmanFilter1D'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_H__
#define __POSITIONKALMANFILTER1D_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
#endif
/* End of code generation (positionKalmanFilter1D.h) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
Executable
+157
View File
@@ -0,0 +1,157 @@
/*
* positionKalmanFilter1D_dT.c
*
* Code generation for function 'positionKalmanFilter1D_dT'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D_dT.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3],
const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update,
const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T
x_aposteriori[3], real32_T P_aposteriori[9])
{
real32_T A[9];
int32_T i;
static const int8_T iv0[3] = { 0, 0, 1 };
real32_T K[3];
real32_T f0;
int32_T i0;
real32_T b_A[9];
int32_T i1;
real32_T P_apriori[9];
static const int8_T iv1[3] = { 1, 0, 0 };
real32_T fv0[3];
real32_T y;
static const int8_T iv2[3] = { 1, 0, 0 };
real32_T S;
int8_T I[9];
/* dynamics */
A[0] = 1.0F;
A[3] = dT;
A[6] = -0.5F * dT * dT;
A[1] = 0.0F;
A[4] = 1.0F;
A[7] = -dT;
for (i = 0; i < 3; i++) {
A[2 + 3 * i] = (real32_T)iv0[i];
}
/* prediction */
K[0] = 0.5F * dT * dT;
K[1] = dT;
K[2] = 0.0F;
for (i = 0; i < 3; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
f0 += A[i + 3 * i0] * x_aposteriori_k[i0];
}
x_aposteriori[i] = f0 + K[i] * u;
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
b_A[i + 3 * i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0];
}
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1];
}
P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0];
}
}
if ((real32_T)fabs(u) < thresh) {
x_aposteriori[1] *= decay;
}
/* update */
if (gps_update == 1) {
f0 = 0.0F;
for (i = 0; i < 3; i++) {
f0 += (real32_T)iv1[i] * x_aposteriori[i];
fv0[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i];
}
}
y = z - f0;
f0 = 0.0F;
for (i = 0; i < 3; i++) {
f0 += fv0[i] * (real32_T)iv2[i];
}
S = f0 + (real32_T)R;
for (i = 0; i < 3; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0];
}
K[i] = f0 / S;
}
for (i = 0; i < 3; i++) {
x_aposteriori[i] += K[i] * y;
}
for (i = 0; i < 9; i++) {
I[i] = 0;
}
for (i = 0; i < 3; i++) {
I[i + 3 * i] = 1;
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i];
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
P_aposteriori[i + 3 * i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0];
}
}
}
} else {
for (i = 0; i < 9; i++) {
P_aposteriori[i] = P_apriori[i];
}
}
}
/* End of code generation (positionKalmanFilter1D_dT.c) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* positionKalmanFilter1D_dT.h
*
* Code generation for function 'positionKalmanFilter1D_dT'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_DT_H__
#define __POSITIONKALMANFILTER1D_DT_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_dT_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
#endif
/* End of code generation (positionKalmanFilter1D_dT.h) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* positionKalmanFilter1D_dT_initialize.c
*
* Code generation for function 'positionKalmanFilter1D_dT_initialize'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D_dT.h"
#include "positionKalmanFilter1D_dT_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_dT_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* positionKalmanFilter1D_dT_initialize.h
*
* Code generation for function 'positionKalmanFilter1D_dT_initialize'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_dT_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D_dT_initialize(void);
#endif
/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* positionKalmanFilter1D_dT_terminate.c
*
* Code generation for function 'positionKalmanFilter1D_dT_terminate'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D_dT.h"
#include "positionKalmanFilter1D_dT_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_dT_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* positionKalmanFilter1D_dT_terminate.h
*
* Code generation for function 'positionKalmanFilter1D_dT_terminate'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_dT_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D_dT_terminate(void);
#endif
/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h
Executable
+16
View File
@@ -0,0 +1,16 @@
/*
* positionKalmanFilter1D_dT_types.h
*
* Code generation for function 'positionKalmanFilter1D_dT'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__
#define __POSITIONKALMANFILTER1D_DT_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (positionKalmanFilter1D_dT_types.h) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* positionKalmanFilter1D_initialize.c
*
* Code generation for function 'positionKalmanFilter1D_initialize'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D.h"
#include "positionKalmanFilter1D_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (positionKalmanFilter1D_initialize.c) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* positionKalmanFilter1D_initialize.h
*
* Code generation for function 'positionKalmanFilter1D_initialize'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__
#define __POSITIONKALMANFILTER1D_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D_initialize(void);
#endif
/* End of code generation (positionKalmanFilter1D_initialize.h) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* positionKalmanFilter1D_terminate.c
*
* Code generation for function 'positionKalmanFilter1D_terminate'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D.h"
#include "positionKalmanFilter1D_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (positionKalmanFilter1D_terminate.c) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* positionKalmanFilter1D_terminate.h
*
* Code generation for function 'positionKalmanFilter1D_terminate'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__
#define __POSITIONKALMANFILTER1D_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D_terminate(void);
#endif
/* End of code generation (positionKalmanFilter1D_terminate.h) */
apps/position_estimator_mc/codegen/positionKalmanFilter1D_types.h
Executable
+16
View File
@@ -0,0 +1,16 @@
/*
* positionKalmanFilter1D_types.h
*
* Code generation for function 'positionKalmanFilter1D'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_TYPES_H__
#define __POSITIONKALMANFILTER1D_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (positionKalmanFilter1D_types.h) */
apps/position_estimator_mc/codegen/randn.c
Executable
+524
View File
@@ -0,0 +1,524 @@
/*
* randn.c
*
* Code generation for function 'randn'
*
* C source code generated on: Tue Feb 19 15:26:32 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "randn.h"
#include "kalman_dlqe3_data.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
static uint32_T d_state[625];
/* Function Declarations */
static real_T b_genrandu(uint32_T mt[625]);
static real_T eml_rand_mt19937ar(uint32_T e_state[625]);
static real_T eml_rand_shr3cong(uint32_T e_state[2]);
static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]);
static void genrandu(uint32_T s, uint32_T *e_state, real_T *r);
static void twister_state_vector(uint32_T mt[625], real_T seed);
/* Function Definitions */
static real_T b_genrandu(uint32_T mt[625])
{
real_T r;
int32_T exitg1;
uint32_T u[2];
boolean_T isvalid;
int32_T k;
boolean_T exitg2;
/* <LEGAL> This is a uniform (0,1) pseudorandom number generator based on: */
/* <LEGAL> */
/* <LEGAL> A C-program for MT19937, with initialization improved 2002/1/26. */
/* <LEGAL> Coded by Takuji Nishimura and Makoto Matsumoto. */
/* <LEGAL> */
/* <LEGAL> Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */
/* <LEGAL> All rights reserved. */
/* <LEGAL> */
/* <LEGAL> Redistribution and use in source and binary forms, with or without */
/* <LEGAL> modification, are permitted provided that the following conditions */
/* <LEGAL> are met: */
/* <LEGAL> */
/* <LEGAL> 1. Redistributions of source code must retain the above copyright */
/* <LEGAL> notice, this list of conditions and the following disclaimer. */
/* <LEGAL> */
/* <LEGAL> 2. Redistributions in binary form must reproduce the above copyright */
/* <LEGAL> notice, this list of conditions and the following disclaimer in the */
/* <LEGAL> documentation and/or other materials provided with the distribution. */
/* <LEGAL> */
/* <LEGAL> 3. The names of its contributors may not be used to endorse or promote */
/* <LEGAL> products derived from this software without specific prior written */
/* <LEGAL> permission. */
/* <LEGAL> */
/* <LEGAL> THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */
/* <LEGAL> "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */
/* <LEGAL> LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */
/* <LEGAL> A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR */
/* <LEGAL> CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */
/* <LEGAL> EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */
/* <LEGAL> PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */
/* <LEGAL> PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */
/* <LEGAL> LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */
/* <LEGAL> NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */
/* <LEGAL> SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
do {
exitg1 = 0;
genrand_uint32_vector(mt, u);
r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)
(u[1] >> 6U));
if (r == 0.0) {
if ((mt[624] >= 1U) && (mt[624] < 625U)) {
isvalid = TRUE;
} else {
isvalid = FALSE;
}
if (isvalid) {
isvalid = FALSE;
k = 1;
exitg2 = FALSE;
while ((exitg2 == FALSE) && (k < 625)) {
if (mt[k - 1] == 0U) {
k++;
} else {
isvalid = TRUE;
exitg2 = TRUE;
}
}
}
if (!isvalid) {
twister_state_vector(mt, 5489.0);
}
} else {
exitg1 = 1;
}
} while (exitg1 == 0);
return r;
}
static real_T eml_rand_mt19937ar(uint32_T e_state[625])
{
real_T r;
int32_T exitg1;
uint32_T u32[2];
int32_T i;
static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068,
0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,
0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,
0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,
0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,
0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,
0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,
0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,
0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,
0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,
0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,
0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,
0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,
0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,
1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,
1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,
1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,
1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,
1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,
1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,
1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,
1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,
1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,
1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,
1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,
1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,
1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,
1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,
1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,
1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,
1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,
1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,
1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,
1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,
1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,
1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,
1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,
1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,
1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,
1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,
1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,
1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,
1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,
1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,
1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,
1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,
1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,
1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,
2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,
2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,
2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,
2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,
2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,
2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,
2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,
2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,
2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,
2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,
2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,
2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,
2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,
2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,
2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,
3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,
3.65415288536101, 3.91075795952492 };
real_T u;
static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108,
0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
0.093365311912691, 0.0911136480663738, 0.0888735920682759,
0.0866451944505581, 0.0844285095703535, 0.082223595813203,
0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
0.000477467764609386 };
real_T x;
do {
exitg1 = 0;
genrand_uint32_vector(e_state, u32);
i = (int32_T)((u32[1] >> 24U) + 1U);
r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &
16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i];
if (fabs(r) <= dv1[i - 1]) {
exitg1 = 1;
} else if (i < 256) {
u = b_genrandu(e_state);
if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) {
exitg1 = 1;
}
} else {
do {
u = b_genrandu(e_state);
x = log(u) * 0.273661237329758;
u = b_genrandu(e_state);
} while (!(-2.0 * log(u) > x * x));
if (r < 0.0) {
r = x - 3.65415288536101;
} else {
r = 3.65415288536101 - x;
}
exitg1 = 1;
}
} while (exitg1 == 0);
return r;
}
static real_T eml_rand_shr3cong(uint32_T e_state[2])
{
real_T r;
uint32_T icng;
uint32_T jsr;
uint32_T ui;
int32_T j;
static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427,
0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948,
0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738,
1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008,
1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122,
1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503,
1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831,
1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713,
2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,
2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 };
real_T x;
real_T y;
real_T s;
icng = 69069U * e_state[0] + 1234567U;
jsr = e_state[1] ^ e_state[1] << 13U;
jsr ^= jsr >> 17U;
jsr ^= jsr << 5U;
ui = icng + jsr;
j = (int32_T)(ui & 63U);
r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1];
if (fabs(r) <= dv0[j]) {
} else {
x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]);
icng = 69069U * icng + 1234567U;
jsr ^= jsr << 13U;
jsr ^= jsr >> 17U;
jsr ^= jsr << 5U;
y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10;
s = x + (0.5 + y);
if (s > 1.301198) {
if (r < 0.0) {
r = 0.4878992 * x - 0.4878992;
} else {
r = 0.4878992 - 0.4878992 * x;
}
} else if (s <= 0.9689279) {
} else {
s = 0.4878992 * x;
x = 0.4878992 - 0.4878992 * x;
if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) {
if (r < 0.0) {
r = -(0.4878992 - s);
} else {
r = 0.4878992 - s;
}
} else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 /
dv0[j + 1] <= exp(-0.5 * r * r)) {
} else {
do {
icng = 69069U * icng + 1234567U;
jsr ^= jsr << 13U;
jsr ^= jsr >> 17U;
jsr ^= jsr << 5U;
x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) /
2.776994;
icng = 69069U * icng + 1234567U;
jsr ^= jsr << 13U;
jsr ^= jsr >> 17U;
jsr ^= jsr << 5U;
} while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) *
2.328306436538696E-10) > x * x));
if (r < 0.0) {
r = x - 2.776994;
} else {
r = 2.776994 - x;
}
}
}
}
e_state[0] = icng;
e_state[1] = jsr;
return r;
}
static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2])
{
int32_T i;
uint32_T mti;
int32_T kk;
uint32_T y;
uint32_T b_y;
uint32_T c_y;
uint32_T d_y;
for (i = 0; i < 2; i++) {
u[i] = 0U;
}
for (i = 0; i < 2; i++) {
mti = mt[624] + 1U;
if (mti >= 625U) {
for (kk = 0; kk < 227; kk++) {
y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U);
if ((int32_T)(y & 1U) == 0) {
b_y = y >> 1U;
} else {
b_y = y >> 1U ^ 2567483615U;
}
mt[kk] = mt[397 + kk] ^ b_y;
}
for (kk = 0; kk < 396; kk++) {
y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U);
if ((int32_T)(y & 1U) == 0) {
c_y = y >> 1U;
} else {
c_y = y >> 1U ^ 2567483615U;
}
mt[227 + kk] = mt[kk] ^ c_y;
}
y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);
if ((int32_T)(y & 1U) == 0) {
d_y = y >> 1U;
} else {
d_y = y >> 1U ^ 2567483615U;
}
mt[623] = mt[396] ^ d_y;
mti = 1U;
}
y = mt[(int32_T)mti - 1];
mt[624] = mti;
y ^= y >> 11U;
y ^= y << 7U & 2636928640U;
y ^= y << 15U & 4022730752U;
y ^= y >> 18U;
u[i] = y;
}
}
static void genrandu(uint32_T s, uint32_T *e_state, real_T *r)
{
int32_T hi;
uint32_T test1;
uint32_T test2;
hi = (int32_T)(s / 127773U);
test1 = 16807U * (s - (uint32_T)hi * 127773U);
test2 = 2836U * (uint32_T)hi;
if (test1 < test2) {
*e_state = (test1 - test2) + 2147483647U;
} else {
*e_state = test1 - test2;
}
*r = (real_T)*e_state * 4.6566128752457969E-10;
}
static void twister_state_vector(uint32_T mt[625], real_T seed)
{
uint32_T r;
int32_T mti;
if (seed < 4.294967296E+9) {
if (seed >= 0.0) {
r = (uint32_T)seed;
} else {
r = 0U;
}
} else if (seed >= 4.294967296E+9) {
r = MAX_uint32_T;
} else {
r = 0U;
}
mt[0] = r;
for (mti = 0; mti < 623; mti++) {
r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);
mt[1 + mti] = r;
}
mt[624] = 624U;
}
real_T randn(void)
{
real_T r;
uint32_T e_state;
real_T t;
real_T b_r;
uint32_T f_state;
if (method == 0U) {
if (b_method == 4U) {
do {
genrandu(b_state, &e_state, &r);
genrandu(e_state, &b_state, &t);
b_r = 2.0 * r - 1.0;
t = 2.0 * t - 1.0;
t = t * t + b_r * b_r;
} while (!(t <= 1.0));
r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);
} else if (b_method == 5U) {
r = eml_rand_shr3cong(c_state);
} else {
if (!state_not_empty) {
memset(&d_state[0], 0, 625U * sizeof(uint32_T));
twister_state_vector(d_state, 5489.0);
state_not_empty = TRUE;
}
r = eml_rand_mt19937ar(d_state);
}
} else if (method == 4U) {
e_state = state[0];
do {
genrandu(e_state, &f_state, &r);
genrandu(f_state, &e_state, &t);
b_r = 2.0 * r - 1.0;
t = 2.0 * t - 1.0;
t = t * t + b_r * b_r;
} while (!(t <= 1.0));
state[0] = e_state;
r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);
} else {
r = eml_rand_shr3cong(state);
}
return r;
}
/* End of code generation (randn.c) */
apps/position_estimator_mc/codegen/randn.h
Executable
+33
View File
@@ -0,0 +1,33 @@
/*
* randn.h
*
* Code generation for function 'randn'
*
* C source code generated on: Tue Feb 19 15:26:32 2013
*
*/
#ifndef __RANDN_H__
#define __RANDN_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern real_T randn(void);
#endif
/* End of code generation (randn.h) */
apps/position_estimator_mc/codegen/rtGetInf.c
Executable
+139
View File
@@ -0,0 +1,139 @@
/*
* rtGetInf.c
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
/*
* Abstract:
* MATLAB for code generation function to initialize non-finite, Inf and MinusInf
*/
#include "rtGetInf.h"
#define NumBitsPerChar 8U
/* Function: rtGetInf ==================================================
* Abstract:
* Initialize rtInf needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real_T rtGetInf(void)
{
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
real_T inf = 0.0;
if (bitsPerReal == 32U) {
inf = rtGetInfF();
} else {
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
union {
LittleEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0x7FF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
inf = tmpVal.fltVal;
break;
}
case BigEndian:
{
union {
BigEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0x7FF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
inf = tmpVal.fltVal;
break;
}
}
}
return inf;
}
/* Function: rtGetInfF ==================================================
* Abstract:
* Initialize rtInfF needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real32_T rtGetInfF(void)
{
IEEESingle infF;
infF.wordL.wordLuint = 0x7F800000U;
return infF.wordL.wordLreal;
}
/* Function: rtGetMinusInf ==================================================
* Abstract:
* Initialize rtMinusInf needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real_T rtGetMinusInf(void)
{
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
real_T minf = 0.0;
if (bitsPerReal == 32U) {
minf = rtGetMinusInfF();
} else {
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
union {
LittleEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0xFFF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
minf = tmpVal.fltVal;
break;
}
case BigEndian:
{
union {
BigEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0xFFF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
minf = tmpVal.fltVal;
break;
}
}
}
return minf;
}
/* Function: rtGetMinusInfF ==================================================
* Abstract:
* Initialize rtMinusInfF needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real32_T rtGetMinusInfF(void)
{
IEEESingle minfF;
minfF.wordL.wordLuint = 0xFF800000U;
return minfF.wordL.wordLreal;
}
/* End of code generation (rtGetInf.c) */
apps/position_estimator_mc/codegen/rtGetInf.h
Executable
+23
View File
@@ -0,0 +1,23 @@
/*
* rtGetInf.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
#ifndef __RTGETINF_H__
#define __RTGETINF_H__
#include <stddef.h>
#include "rtwtypes.h"
#include "rt_nonfinite.h"
extern real_T rtGetInf(void);
extern real32_T rtGetInfF(void);
extern real_T rtGetMinusInf(void);
extern real32_T rtGetMinusInfF(void);
#endif
/* End of code generation (rtGetInf.h) */
apps/position_estimator_mc/codegen/rtGetNaN.c
Executable
+96
View File
@@ -0,0 +1,96 @@
/*
* rtGetNaN.c
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
/*
* Abstract:
* MATLAB for code generation function to initialize non-finite, NaN
*/
#include "rtGetNaN.h"
#define NumBitsPerChar 8U
/* Function: rtGetNaN ==================================================
* Abstract:
* Initialize rtNaN needed by the generated code.
* NaN is initialized as non-signaling. Assumes IEEE.
*/
real_T rtGetNaN(void)
{
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
real_T nan = 0.0;
if (bitsPerReal == 32U) {
nan = rtGetNaNF();
} else {
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
union {
LittleEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0xFFF80000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
nan = tmpVal.fltVal;
break;
}
case BigEndian:
{
union {
BigEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
nan = tmpVal.fltVal;
break;
}
}
}
return nan;
}
/* Function: rtGetNaNF ==================================================
* Abstract:
* Initialize rtNaNF needed by the generated code.
* NaN is initialized as non-signaling. Assumes IEEE.
*/
real32_T rtGetNaNF(void)
{
IEEESingle nanF = { { 0 } };
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
nanF.wordL.wordLuint = 0xFFC00000U;
break;
}
case BigEndian:
{
nanF.wordL.wordLuint = 0x7FFFFFFFU;
break;
}
}
return nanF.wordL.wordLreal;
}
/* End of code generation (rtGetNaN.c) */
apps/position_estimator_mc/codegen/rtGetNaN.h
Executable
+21
View File
@@ -0,0 +1,21 @@
/*
* rtGetNaN.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
#ifndef __RTGETNAN_H__
#define __RTGETNAN_H__
#include <stddef.h>
#include "rtwtypes.h"
#include "rt_nonfinite.h"
extern real_T rtGetNaN(void);
extern real32_T rtGetNaNF(void);
#endif
/* End of code generation (rtGetNaN.h) */
apps/position_estimator_mc/codegen/rt_nonfinite.c
Executable
+87
View File
@@ -0,0 +1,87 @@
/*
* rt_nonfinite.c
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
/*
* Abstract:
* MATLAB for code generation function to initialize non-finites,
* (Inf, NaN and -Inf).
*/
#include "rt_nonfinite.h"
#include "rtGetNaN.h"
#include "rtGetInf.h"
real_T rtInf;
real_T rtMinusInf;
real_T rtNaN;
real32_T rtInfF;
real32_T rtMinusInfF;
real32_T rtNaNF;
/* Function: rt_InitInfAndNaN ==================================================
* Abstract:
* Initialize the rtInf, rtMinusInf, and rtNaN needed by the
* generated code. NaN is initialized as non-signaling. Assumes IEEE.
*/
void rt_InitInfAndNaN(size_t realSize)
{
(void) (realSize);
rtNaN = rtGetNaN();
rtNaNF = rtGetNaNF();
rtInf = rtGetInf();
rtInfF = rtGetInfF();
rtMinusInf = rtGetMinusInf();
rtMinusInfF = rtGetMinusInfF();
}
/* Function: rtIsInf ==================================================
* Abstract:
* Test if value is infinite
*/
boolean_T rtIsInf(real_T value)
{
return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
}
/* Function: rtIsInfF =================================================
* Abstract:
* Test if single-precision value is infinite
*/
boolean_T rtIsInfF(real32_T value)
{
return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
}
/* Function: rtIsNaN ==================================================
* Abstract:
* Test if value is not a number
*/
boolean_T rtIsNaN(real_T value)
{
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
return _isnan(value)? TRUE:FALSE;
#else
return (value!=value)? 1U:0U;
#endif
}
/* Function: rtIsNaNF =================================================
* Abstract:
* Test if single-precision value is not a number
*/
boolean_T rtIsNaNF(real32_T value)
{
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
return _isnan((real_T)value)? true:false;
#else
return (value!=value)? 1U:0U;
#endif
}
/* End of code generation (rt_nonfinite.c) */
apps/position_estimator_mc/codegen/rt_nonfinite.h
Executable
+53
View File
@@ -0,0 +1,53 @@
/*
* rt_nonfinite.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
#ifndef __RT_NONFINITE_H__
#define __RT_NONFINITE_H__
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
#include <float.h>
#endif
#include <stddef.h>
#include "rtwtypes.h"
extern real_T rtInf;
extern real_T rtMinusInf;
extern real_T rtNaN;
extern real32_T rtInfF;
extern real32_T rtMinusInfF;
extern real32_T rtNaNF;
extern void rt_InitInfAndNaN(size_t realSize);
extern boolean_T rtIsInf(real_T value);
extern boolean_T rtIsInfF(real32_T value);
extern boolean_T rtIsNaN(real_T value);
extern boolean_T rtIsNaNF(real32_T value);
typedef struct {
struct {
uint32_T wordH;
uint32_T wordL;
} words;
} BigEndianIEEEDouble;
typedef struct {
struct {
uint32_T wordL;
uint32_T wordH;
} words;
} LittleEndianIEEEDouble;
typedef struct {
union {
real32_T wordLreal;
uint32_T wordLuint;
} wordL;
} IEEESingle;
#endif
/* End of code generation (rt_nonfinite.h) */
apps/position_estimator_mc/codegen/rtwtypes.h
Executable
+159
View File
@@ -0,0 +1,159 @@
/*
* rtwtypes.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
#ifndef __RTWTYPES_H__
#define __RTWTYPES_H__
#ifndef TRUE
# define TRUE (1U)
#endif
#ifndef FALSE
# define FALSE (0U)
#endif
#ifndef __TMWTYPES__
#define __TMWTYPES__
#include <limits.h>
/*=======================================================================*
* Target hardware information
* Device type: Generic->MATLAB Host Computer
* 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: off
*=======================================================================*/
/*=======================================================================*
* 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) */
apps/position_estimator_mc/kalman_dlqe1.m
Executable
+3
View File
@@ -0,0 +1,3 @@
function [x_aposteriori] = kalman_dlqe1(A,C,K,x_aposteriori_k,z)
x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
end
apps/position_estimator_mc/kalman_dlqe2.m
Executable
+9
View File
@@ -0,0 +1,9 @@
function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z)
st = 1/2*dt^2;
A = [1,dt,st;
0,1,dt;
0,0,1];
C=[1,0,0];
K = [k1;k2;k3];
x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
end
apps/position_estimator_mc/kalman_dlqe3.m
Executable
+17
View File
@@ -0,0 +1,17 @@
function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma)
st = 1/2*dt^2;
A = [1,dt,st;
0,1,dt;
0,0,1];
C=[1,0,0];
K = [k1;k2;k3];
if addNoise==1
noise = sigma*randn(1,1);
z = z + noise;
end
if(posUpdate)
x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
else
x_aposteriori=A*x_aposteriori_k;
end
end
apps/position_estimator_mc/positionKalmanFilter1D.m
Executable
+19
View File
@@ -0,0 +1,19 @@
function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
%prediction
x_apriori=A*x_aposteriori_k+B*u;
P_apriori=A*P_aposteriori_k*A'+Q;
if abs(u)<thresh
x_apriori(2)=decay*x_apriori(2);
end
%update
if gps_update==1
y=z-C*x_apriori;
S=C*P_apriori*C'+R;
K=(P_apriori*C')/S;
x_aposteriori=x_apriori+K*y;
P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
else
x_aposteriori=x_apriori;
P_aposteriori=P_apriori;
end
end
apps/position_estimator_mc/positionKalmanFilter1D_dT.m
Executable
+26
View File
@@ -0,0 +1,26 @@
function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D_dT(dT,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
%dynamics
A = [1 dT -0.5*dT*dT;
0 1 -dT;
0 0 1];
B = [0.5*dT*dT; dT; 0];
C = [1 0 0];
%prediction
x_apriori=A*x_aposteriori_k+B*u;
P_apriori=A*P_aposteriori_k*A'+Q;
if abs(u)<thresh
x_apriori(2)=decay*x_apriori(2);
end
%update
if gps_update==1
y=z-C*x_apriori;
S=C*P_apriori*C'+R;
K=(P_apriori*C')/S;
x_aposteriori=x_apriori+K*y;
P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
else
x_aposteriori=x_apriori;
P_aposteriori=P_apriori;
end
end
apps/position_estimator_mc/position_estimator_mc_main.c
Executable
+510
View File
@@ -0,0 +1,510 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Damian Aregger <daregger@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 position_estimator_main.c
* Model-identification based position estimator for multirotors
*/
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <string.h>
#include <nuttx/config.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
#include <poll.h>
#include <systemlib/geo/geo.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
#include "position_estimator_mc_params.h"
//#include <uORB/topics/debug_key_value.h>
#include "codegen/kalman_dlqe2.h"
#include "codegen/kalman_dlqe2_initialize.h"
#include "codegen/kalman_dlqe3.h"
#include "codegen/kalman_dlqe3_initialize.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int position_estimator_mc_task; /**< Handle of deamon task / thread */
__EXPORT int position_estimator_mc_main(int argc, char *argv[]);
int position_estimator_mc_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
warnx("%s\n", reason);
warnx("usage: position_estimator_mc {start|stop|status}");
exit(1);
}
/**
* The position_estimator_mc_thread 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 task_create().
*/
int position_estimator_mc_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("position_estimator_mc already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
position_estimator_mc_task = task_spawn("position_estimator_mc",
SCHED_RR,
SCHED_PRIORITY_MAX - 5,
4096,
position_estimator_mc_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("position_estimator_mc is running");
} else {
warnx("position_estimator_mc not started");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
/****************************************************************************
* main
****************************************************************************/
int position_estimator_mc_thread_main(int argc, char *argv[])
{
/* welcome user */
warnx("[position_estimator_mc] started");
int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[position_estimator_mc] started");
/* initialize values */
float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */
// float rotMatrix[4] = {1.0f, 0.0f, 0.0f, 1.0f};
float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f};
float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f};
float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f};
// XXX this is terribly wrong and should actual dT instead
const float dT_const_50 = 1.0f/50.0f;
float addNoise = 0.0f;
float sigma = 0.0f;
//computed from dlqe in matlab
const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f};
// XXX implement baro filter
const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f};
float K[3] = {0.0f, 0.0f, 0.0f};
int baro_loop_cnt = 0;
int baro_loop_end = 70; /* measurement for 1 second */
float p0_Pa = 0.0f; /* to determin while start up */
float rho0 = 1.293f; /* standard pressure */
const float const_earth_gravity = 9.81f;
float posX = 0.0f;
float posY = 0.0f;
float posZ = 0.0f;
double lat_current;
double lon_current;
float alt_current;
float gps_origin_altitude = 0.0f;
/* Initialize filter */
kalman_dlqe2_initialize();
kalman_dlqe3_initialize();
/* declare and safely initialize all structs */
struct sensor_combined_s sensor;
memset(&sensor, 0, sizeof(sensor));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */
struct vehicle_vicon_position_s vicon_pos;
memset(&vicon_pos, 0, sizeof(vicon_pos));
struct actuator_controls_effective_s act_eff;
memset(&act_eff, 0, sizeof(act_eff));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
struct vehicle_local_position_s local_pos_est;
memset(&local_pos_est, 0, sizeof(local_pos_est));
struct vehicle_global_position_s global_pos_est;
memset(&global_pos_est, 0, sizeof(global_pos_est));
/* subscribe */
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
int sub_params = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0));
int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* advertise */
orb_advert_t local_pos_est_pub = 0;
orb_advert_t global_pos_est_pub = 0;
struct position_estimator_mc_params pos1D_params;
struct position_estimator_mc_param_handles pos1D_param_handles;
/* initialize parameter handles */
parameters_init(&pos1D_param_handles);
bool flag_use_gps = false;
bool flag_use_baro = false;
bool flag_baro_initialized = false; /* in any case disable baroINITdone */
/* FIRST PARAMETER READ at START UP*/
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */
/* FIRST PARAMETER UPDATE */
parameters_update(&pos1D_param_handles, &pos1D_params);
flag_use_baro = pos1D_params.baro;
sigma = pos1D_params.sigma;
addNoise = pos1D_params.addNoise;
/* END FIRST PARAMETER UPDATE */
/* try to grab a vicon message - if it fails, go for GPS. */
/* make sure the next orb_check() can't return true unless we get a timely update */
orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
/* allow 200 ms for vicon to come in */
usleep(200000);
/* check if we got vicon */
bool update_check;
orb_check(vicon_pos_sub, &update_check);
/* if no update was available, use GPS */
flag_use_gps = !update_check;
if (flag_use_gps) {
mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked");
/* wait until gps signal turns valid, only then can we initialize the projection */
// XXX magic number
float hdop_threshold_m = 4.0f;
float vdop_threshold_m = 8.0f;
/*
* If horizontal dilution of precision (hdop / eph)
* and vertical diluation of precision (vdop / epv)
* are below a certain threshold (e.g. 4 m), AND
* home position is not yet set AND the last GPS
* GPS measurement is not older than two seconds AND
* the system is currently not armed, set home
* position to the current position.
*/
while (!(gps.fix_type == 3
&& (gps.eph_m < hdop_threshold_m)
&& (gps.epv_m < vdop_threshold_m)
&& (hrt_absolute_time() - gps.timestamp_position < 2000000))) {
struct pollfd fds1[2] = {
{ .fd = vehicle_gps_sub, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN },
};
/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
* this choice is critical, since the vehicle status might not
* actually change, if this app is started after GPS lock was
* aquired.
*/
if (poll(fds1, 2, 5000)) {
if (fds1[0].revents & POLLIN){
/* Read gps position */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
}
if (fds1[1].revents & POLLIN){
/* Read out parameters to check for an update there, e.g. useGPS variable */
/* read from param to clear updated flag */
struct parameter_update_s updated;
orb_copy(ORB_ID(parameter_update), sub_params, &updated);
/* update parameters */
parameters_update(&pos1D_param_handles, &pos1D_params);
}
}
static int printcounter = 0;
if (printcounter == 100) {
printcounter = 0;
warnx("[pos_est_mc] wait for GPS fix");
}
printcounter++;
}
/* get gps value for first initialization */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
lat_current = ((double)(gps.lat)) * 1e-7d;
lon_current = ((double)(gps.lon)) * 1e-7d;
alt_current = gps.alt * 1e-3f;
gps_origin_altitude = alt_current;
/* initialize coordinates */
map_projection_init(lat_current, lon_current);
/* publish global position messages only after first GPS message */
printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current);
} else {
mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON");
/* onboard calculated position estimations */
}
thread_running = true;
struct pollfd fds2[3] = {
{ .fd = vehicle_gps_sub, .events = POLLIN },
{ .fd = vicon_pos_sub, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN },
};
bool vicon_updated = false;
bool gps_updated = false;
/**< main_loop */
while (!thread_should_exit) {
int ret = poll(fds2, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate
if (ret < 0) {
/* poll error */
} else {
if (fds2[2].revents & POLLIN){
/* new parameter */
/* read from param to clear updated flag */
struct parameter_update_s updated;
orb_copy(ORB_ID(parameter_update), sub_params, &updated);
/* update parameters */
parameters_update(&pos1D_param_handles, &pos1D_params);
flag_use_baro = pos1D_params.baro;
sigma = pos1D_params.sigma;
addNoise = pos1D_params.addNoise;
}
vicon_updated = false; /* default is no vicon_updated */
if (fds2[1].revents & POLLIN) {
/* new vicon position */
orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
posX = vicon_pos.x;
posY = vicon_pos.y;
posZ = vicon_pos.z;
vicon_updated = true; /* set flag for vicon update */
} /* end of poll call for vicon updates */
gps_updated = false;
if (fds2[0].revents & POLLIN) {
/* new GPS value */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
/* Project gps lat lon (Geographic coordinate system) to plane*/
map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1]));
posX = z[0];
posY = z[1];
posZ = (float)(gps.alt * 1e-3f);
gps_updated = true;
}
/* Main estimator loop */
orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff);
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor);
// barometric pressure estimation at start up
if (!flag_baro_initialized){
// mean calculation over several measurements
if (baro_loop_cnt<baro_loop_end) {
p0_Pa += (sensor.baro_pres_mbar*100);
baro_loop_cnt++;
} else {
p0_Pa /= (float)(baro_loop_cnt);
flag_baro_initialized = true;
char *baro_m_start = "barometer initialized with p0 = ";
char p0_char[15];
sprintf(p0_char, "%8.2f", (double)(p0_Pa/100));
char *baro_m_end = " mbar";
char str[80];
strcpy(str,baro_m_start);
strcat(str,p0_char);
strcat(str,baro_m_end);
mavlink_log_info(mavlink_fd, str);
}
}
if (flag_use_gps) {
/* initialize map projection with the last estimate (not at full rate) */
if (gps.fix_type > 2) {
/* x-y-position/velocity estimation in earth frame = gps frame */
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori);
memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori);
memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
/* z-position/velocity estimation in earth frame = vicon frame */
float z_est = 0.0f;
if (flag_baro_initialized && flag_use_baro) {
z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity);
K[0] = K_vicon_50Hz[0];
K[1] = K_vicon_50Hz[1];
K[2] = K_vicon_50Hz[2];
gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */
} else {
z_est = posZ;
K[0] = K_vicon_50Hz[0];
K[1] = K_vicon_50Hz[1];
K[2] = K_vicon_50Hz[2];
}
kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori);
memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
local_pos_est.x = x_x_aposteriori_k[0];
local_pos_est.vx = x_x_aposteriori_k[1];
local_pos_est.y = x_y_aposteriori_k[0];
local_pos_est.vy = x_y_aposteriori_k[1];
local_pos_est.z = x_z_aposteriori_k[0];
local_pos_est.vz = x_z_aposteriori_k[1];
local_pos_est.timestamp = hrt_absolute_time();
if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) {
/* publish local position estimate */
if (local_pos_est_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
} else {
local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
}
/* publish on GPS updates */
if (gps_updated) {
double lat, lon;
float alt = z_est + gps_origin_altitude;
map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon);
global_pos_est.lat = lat;
global_pos_est.lon = lon;
global_pos_est.alt = alt;
if (global_pos_est_pub > 0) {
orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est);
} else {
global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est);
}
}
}
}
} else {
/* x-y-position/velocity estimation in earth frame = vicon frame */
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori);
memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori);
memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
/* z-position/velocity estimation in earth frame = vicon frame */
float z_est = 0.0f;
float local_sigma = 0.0f;
if (flag_baro_initialized && flag_use_baro) {
z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity);
K[0] = K_vicon_50Hz[0];
K[1] = K_vicon_50Hz[1];
K[2] = K_vicon_50Hz[2];
vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */
local_sigma = 0.0f; /* don't add noise on barometer in any case */
} else {
z_est = posZ;
K[0] = K_vicon_50Hz[0];
K[1] = K_vicon_50Hz[1];
K[2] = K_vicon_50Hz[2];
local_sigma = sigma;
}
kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori);
memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
local_pos_est.x = x_x_aposteriori_k[0];
local_pos_est.vx = x_x_aposteriori_k[1];
local_pos_est.y = x_y_aposteriori_k[0];
local_pos_est.vy = x_y_aposteriori_k[1];
local_pos_est.z = x_z_aposteriori_k[0];
local_pos_est.vz = x_z_aposteriori_k[1];
local_pos_est.timestamp = hrt_absolute_time();
if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
}
}
} /* end of poll return value check */
}
printf("[pos_est_mc] exiting.\n");
mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting");
thread_running = false;
return 0;
}
apps/position_estimator_mc/position_estimator_mc_params.c
Executable
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Damian Aregger <daregger@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 position_estimator_mc_params.c
*
* Parameters for position_estimator_mc
*/
#include "position_estimator_mc_params.h"
/* Kalman Filter covariances */
/* gps measurement noise standard deviation */
PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f);
PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f);
PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f);
PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f);
int parameters_init(struct position_estimator_mc_param_handles *h)
{
h->addNoise = param_find("POS_EST_ADDN");
h->sigma = param_find("POS_EST_SIGMA");
h->r = param_find("POS_EST_R");
h->baro_param_handle = param_find("POS_EST_BARO");
return OK;
}
int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p)
{
param_get(h->addNoise, &(p->addNoise));
param_get(h->sigma, &(p->sigma));
param_get(h->r, &(p->R));
param_get(h->baro_param_handle, &(p->baro));
return OK;
}
apps/position_estimator_mc/position_estimator_mc_params.h
Executable
+69
View File
@@ -0,0 +1,69 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Damian Aregger <daregger@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 position_estimator_mc_params.h
*
* Parameters for Position Estimator
*/
#include <systemlib/param/param.h>
struct position_estimator_mc_params {
float addNoise;
float sigma;
float R;
int baro; /* consider barometer */
};
struct position_estimator_mc_param_handles {
param_t addNoise;
param_t sigma;
param_t r;
param_t baro_param_handle;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct position_estimator_mc_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p);
nuttx/configs/px4fmu/nsh/appconfig
+1 -1
View File
@@ -97,7 +97,7 @@ ifneq ($(CONFIG_APM),y)
CONFIGURED_APPS += ardrone_interface
CONFIGURED_APPS += multirotor_att_control
CONFIGURED_APPS += multirotor_pos_control
#CONFIGURED_APPS += fixedwing_control
CONFIGURED_APPS += position_estimator_mc
CONFIGURED_APPS += fixedwing_att_control
CONFIGURED_APPS += fixedwing_pos_control
CONFIGURED_APPS += position_estimator