mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-07 01:04:19 +08:00
Butchered position estimator from Damian Aregger into shape, publishes now global position estimate as well. Compiling, needs HIL testing
This commit is contained in:
Lorenz Meier
@@ -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
|
||||
+58
@@ -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) */
|
||||
+30
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
+16
@@ -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) */
|
||||
+119
@@ -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) */
|
||||
+32
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
+16
@@ -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) */
|
||||
+137
@@ -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) */
|
||||
+33
@@ -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) */
|
||||
+32
@@ -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) */
|
||||
+38
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
+16
@@ -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) */
|
||||
+136
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
Executable
+524
File diff suppressed because it is too large
Load Diff
Executable
+33
@@ -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) */
|
||||
+139
@@ -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) */
|
||||
+23
@@ -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) */
|
||||
+96
@@ -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) */
|
||||
+21
@@ -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) */
|
||||
+87
@@ -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) */
|
||||
+53
@@ -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) */
|
||||
+159
@@ -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) */
|
||||
Executable
+3
@@ -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
|
||||
Executable
+9
@@ -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
|
||||
Executable
+17
@@ -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
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user