mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-16 08:40:35 +08:00
Updated EKF filter, untested
This commit is contained in:
Lorenz Meier
@@ -44,7 +44,10 @@ CSRCS = attitude_estimator_ekf_main.c \
|
||||
codegen/rt_nonfinite.c \
|
||||
codegen/rtGetInf.c \
|
||||
codegen/rtGetNaN.c \
|
||||
codegen/norm.c
|
||||
codegen/norm.c \
|
||||
codegen/find.c \
|
||||
codegen/diag.c \
|
||||
codegen/cross.c
|
||||
|
||||
# XXX this is *horribly* broken
|
||||
INCLUDES += $(TOPDIR)/../mavlink/include/mavlink
|
||||
|
||||
@@ -80,21 +80,35 @@ static float dt = 1;
|
||||
/* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */
|
||||
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
|
||||
static float z_k[9] = {0}; /**< Measurement vector */
|
||||
static float x_aposteriori[12] = {0}; /**< */
|
||||
static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f
|
||||
static float x_aposteriori_k[9] = {0}; /**< */
|
||||
static float x_aposteriori[9] = {0};
|
||||
static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
};
|
||||
static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
}; /**< init: diagonal matrix with big values */
|
||||
static float knownConst[7] = {1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
|
||||
static float knownConst[20] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
|
||||
static float Rot_matrix[9] = {1.f, 0, 0,
|
||||
0, 1.f, 0,
|
||||
0, 0, 1.f
|
||||
@@ -203,13 +217,31 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
|
||||
/*
|
||||
* function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
|
||||
*/
|
||||
|
||||
//extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9],
|
||||
// const int32_T z_k_sizes[1], const real32_T u[4],
|
||||
// const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81],
|
||||
// const real32_T knownConst[20], real32_T eulerAngles[3],
|
||||
// real32_T Rot_matrix[9], real32_T x_aposteriori[9],
|
||||
// real32_T P_aposteriori[81]);
|
||||
|
||||
int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1};
|
||||
float euler[3];
|
||||
int32_t z_k_sizes = 9;
|
||||
float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
uint64_t timing_start = hrt_absolute_time();
|
||||
attitudeKalmanfilter(dt, z_k, x_aposteriori, P_aposteriori, knownConst, Rot_matrix, x_aposteriori, P_aposteriori);
|
||||
attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
|
||||
Rot_matrix, x_aposteriori, P_aposteriori);
|
||||
/* swap values for next iteration */
|
||||
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
|
||||
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
|
||||
uint64_t timing_diff = hrt_absolute_time() - timing_start;
|
||||
|
||||
/* print rotation matrix every 200th time */
|
||||
if (printcounter % 200 == 0) {
|
||||
printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
|
||||
printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", euler[0], euler[1], euler[2]);
|
||||
printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
|
||||
(int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
|
||||
(int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:22 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -14,6 +14,8 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
@@ -27,6 +29,6 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
|
||||
extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[9], real32_T P_aposteriori[81]);
|
||||
#endif
|
||||
/* End of code generation (attitudeKalmanfilter.h) */
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_initialize'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:23 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_initialize'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -14,6 +14,8 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
@@ -1,11 +0,0 @@
|
||||
@echo off
|
||||
call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64
|
||||
|
||||
cd .
|
||||
nmake -f attitudeKalmanfilter_rtw.mk GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
|
||||
@if errorlevel 1 goto error_exit
|
||||
exit /B 0
|
||||
|
||||
:error_exit
|
||||
echo The make command returned an error of %errorlevel%
|
||||
An_error_occurred_during_the_call_to_make
|
||||
@@ -1,328 +0,0 @@
|
||||
# Copyright 1994-2010 The MathWorks, Inc.
|
||||
#
|
||||
# File : xrt_vcx64.tmf $Revision: 1.1.6.1 $
|
||||
#
|
||||
# Abstract:
|
||||
# Code generation template makefile for building a Windows-based,
|
||||
# stand-alone real-time version of MATLAB-code using generated C/C++
|
||||
# code and the
|
||||
# Microsoft Visual C/C++ compiler version 8, 9, 10 for x64
|
||||
#
|
||||
# Note that this template is automatically customized by the
|
||||
# code generation build procedure to create "<model>.mk"
|
||||
#
|
||||
# The following defines can be used to modify the behavior of the
|
||||
# build:
|
||||
#
|
||||
# OPT_OPTS - Optimization option. See DEFAULT_OPT_OPTS in
|
||||
# vctools.mak for default.
|
||||
# OPTS - User specific options.
|
||||
# CPP_OPTS - C++ compiler options.
|
||||
# USER_SRCS - Additional user sources, such as files needed by
|
||||
# S-functions.
|
||||
# USER_INCLUDES - Additional include paths
|
||||
# (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2")
|
||||
#
|
||||
# To enable debugging:
|
||||
# set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc)
|
||||
# modify tmf LDFLAGS to include /DEBUG
|
||||
#
|
||||
|
||||
#------------------------ Macros read by make_rtw -----------------------------
|
||||
#
|
||||
# The following macros are read by the code generation build procedure:
|
||||
#
|
||||
# MAKECMD - This is the command used to invoke the make utility
|
||||
# HOST - What platform this template makefile is targeted for
|
||||
# (i.e. PC or UNIX)
|
||||
# BUILD - Invoke make from the code generation build procedure
|
||||
# (yes/no)?
|
||||
# SYS_TARGET_FILE - Name of system target file.
|
||||
|
||||
MAKECMD = nmake
|
||||
HOST = PC
|
||||
BUILD = yes
|
||||
SYS_TARGET_FILE = ert.tlc
|
||||
BUILD_SUCCESS = ^#^#^# Created
|
||||
COMPILER_TOOL_CHAIN = vcx64
|
||||
|
||||
#---------------------- Tokens expanded by make_rtw ---------------------------
|
||||
#
|
||||
# The following tokens, when wrapped with "|>" and "<|" are expanded by the
|
||||
# code generation build procedure.
|
||||
#
|
||||
# MODEL_NAME - Name of the Simulink block diagram
|
||||
# MODEL_MODULES - Any additional generated source modules
|
||||
# MAKEFILE_NAME - Name of makefile created from template makefile <model>.mk
|
||||
# MATLAB_ROOT - Path to where MATLAB is installed.
|
||||
# MATLAB_BIN - Path to MATLAB executable.
|
||||
# S_FUNCTIONS - List of S-functions.
|
||||
# S_FUNCTIONS_LIB - List of S-functions libraries to link.
|
||||
# SOLVER - Solver source file name
|
||||
# NUMST - Number of sample times
|
||||
# TID01EQ - yes (1) or no (0): Are sampling rates of continuous task
|
||||
# (tid=0) and 1st discrete task equal.
|
||||
# NCSTATES - Number of continuous states
|
||||
# BUILDARGS - Options passed in at the command line.
|
||||
# MULTITASKING - yes (1) or no (0): Is solver mode multitasking
|
||||
# EXT_MODE - yes (1) or no (0): Build for external mode
|
||||
# TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode
|
||||
# testing.
|
||||
# EXTMODE_TRANSPORT - Index of transport mechanism (e.g. tcpip, serial) for extmode
|
||||
# EXTMODE_STATIC - yes (1) or no (0): Use static instead of dynamic mem alloc.
|
||||
# EXTMODE_STATIC_SIZE - Size of static memory allocation buffer.
|
||||
|
||||
MODEL = attitudeKalmanfilter
|
||||
MODULES = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c
|
||||
MAKEFILE = attitudeKalmanfilter_rtw.mk
|
||||
MATLAB_ROOT = C:\Program Files\MATLAB\R2011a
|
||||
ALT_MATLAB_ROOT = C:\PROGRA~1\MATLAB\R2011a
|
||||
MATLAB_BIN = C:\Program Files\MATLAB\R2011a\bin
|
||||
ALT_MATLAB_BIN = C:\PROGRA~1\MATLAB\R2011a\bin
|
||||
S_FUNCTIONS =
|
||||
S_FUNCTIONS_LIB =
|
||||
SOLVER =
|
||||
NUMST = 1
|
||||
TID01EQ = 0
|
||||
NCSTATES = 0
|
||||
BUILDARGS = GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
|
||||
MULTITASKING = 0
|
||||
EXT_MODE = 0
|
||||
TMW_EXTMODE_TESTING = 0
|
||||
EXTMODE_TRANSPORT = 0
|
||||
EXTMODE_STATIC = 0
|
||||
EXTMODE_STATIC_SIZE = 1000000
|
||||
|
||||
MODELREFS =
|
||||
SHARED_SRC =
|
||||
SHARED_SRC_DIR =
|
||||
SHARED_BIN_DIR =
|
||||
SHARED_LIB =
|
||||
TARGET_LANG_EXT = c
|
||||
OPTIMIZATION_FLAGS = /O2 /Oy-
|
||||
ADDITIONAL_LDFLAGS =
|
||||
|
||||
#--------------------------- Model and reference models -----------------------
|
||||
MODELLIB = attitudeKalmanfilter.lib
|
||||
MODELREF_LINK_LIBS =
|
||||
MODELREF_LINK_RSPFILE = attitudeKalmanfilter_ref.rsp
|
||||
MODELREF_INC_PATH =
|
||||
RELATIVE_PATH_TO_ANCHOR = F:\CODEGE~1
|
||||
MODELREF_TARGET_TYPE = RTW
|
||||
|
||||
!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)"
|
||||
MATLAB_ROOT = $(ALT_MATLAB_ROOT)
|
||||
!endif
|
||||
!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)"
|
||||
MATLAB_BIN = $(ALT_MATLAB_BIN)
|
||||
!endif
|
||||
|
||||
#--------------------------- Tool Specifications ------------------------------
|
||||
|
||||
CPU = AMD64
|
||||
!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak
|
||||
APPVER = 5.02
|
||||
|
||||
PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl
|
||||
#------------------------------ Include/Lib Path ------------------------------
|
||||
|
||||
MATLAB_INCLUDES = $(MATLAB_ROOT)\simulink\include
|
||||
MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include
|
||||
MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src
|
||||
MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common
|
||||
|
||||
# Additional file include paths
|
||||
|
||||
MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter
|
||||
MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter
|
||||
|
||||
INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH)
|
||||
|
||||
!if "$(SHARED_SRC_DIR)" != ""
|
||||
INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR)
|
||||
!endif
|
||||
|
||||
#------------------------ External mode ---------------------------------------
|
||||
# Uncomment -DVERBOSE to have information printed to stdout
|
||||
# To add a new transport layer, see the comments in
|
||||
# <matlabroot>/toolbox/simulink/simulink/extmode_transports.m
|
||||
!if $(EXT_MODE) == 1
|
||||
EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE
|
||||
!if $(EXTMODE_TRANSPORT) == 0 #tcpip
|
||||
EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c
|
||||
EXT_LIB = wsock32.lib
|
||||
!endif
|
||||
!if $(EXTMODE_TRANSPORT) == 1 #serial_win32
|
||||
EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c
|
||||
EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c
|
||||
EXT_LIB =
|
||||
!endif
|
||||
!if $(TMW_EXTMODE_TESTING) == 1
|
||||
EXT_SRC = $(EXT_SRC) ext_test.c
|
||||
EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING
|
||||
!endif
|
||||
!if $(EXTMODE_STATIC) == 1
|
||||
EXT_SRC = $(EXT_SRC) mem_mgr.c
|
||||
EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE)
|
||||
!endif
|
||||
!else
|
||||
EXT_SRC =
|
||||
EXT_CC_OPTS =
|
||||
EXT_LIB =
|
||||
!endif
|
||||
|
||||
#------------------------ rtModel ----------------------------------------------
|
||||
|
||||
RTM_CC_OPTS = -DUSE_RTMODEL
|
||||
|
||||
#----------------- Compiler and Linker Options --------------------------------
|
||||
|
||||
# Optimization Options
|
||||
# Set OPT_OPTS=-Zi for debugging (may depend on compiler version)
|
||||
OPT_OPTS = $(DEFAULT_OPT_OPTS)
|
||||
|
||||
# General User Options
|
||||
OPTS =
|
||||
|
||||
!if "$(OPTIMIZATION_FLAGS)" != ""
|
||||
CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS)
|
||||
!else
|
||||
CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS)
|
||||
!endif
|
||||
|
||||
CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \
|
||||
-DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \
|
||||
-DMT=$(MULTITASKING) -DHAVESTDIO
|
||||
|
||||
# Uncomment this line to move warning level to W4
|
||||
# cflags = $(cflags:W3=W4)
|
||||
CFLAGS = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
|
||||
$(cflags) $(cvarsmt) /wd4996
|
||||
CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
|
||||
$(cflags) $(cvarsmt) /wd4996 /EHsc-
|
||||
LDFLAGS = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS)
|
||||
|
||||
# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib
|
||||
|
||||
#----------------------------- Source Files -----------------------------------
|
||||
|
||||
|
||||
#Standalone executable
|
||||
!if "$(MODELREF_TARGET_TYPE)" == "NONE"
|
||||
PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe
|
||||
REQ_SRCS = $(MODULES) $(EXT_SRC)
|
||||
|
||||
#Model Reference Target
|
||||
!else
|
||||
PRODUCT = $(MODELLIB)
|
||||
REQ_SRCS = $(MODULES) $(EXT_SRC)
|
||||
!endif
|
||||
|
||||
USER_SRCS =
|
||||
|
||||
SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS)
|
||||
OBJS_CPP_UPPER = $(SRCS:.CPP=.obj)
|
||||
OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj)
|
||||
OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj)
|
||||
OBJS = $(OBJS_C_UPPER:.c=.obj)
|
||||
SHARED_OBJS = $(SHARED_SRC:.c=.obj)
|
||||
# ------------------------- Additional Libraries ------------------------------
|
||||
|
||||
LIBS =
|
||||
|
||||
|
||||
LIBS = $(LIBS)
|
||||
|
||||
# ---------------------------- Linker Script ----------------------------------
|
||||
|
||||
CMD_FILE = $(MODEL).lnk
|
||||
GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl
|
||||
|
||||
#--------------------------------- Rules --------------------------------------
|
||||
all: set_environment_variables $(PRODUCT)
|
||||
|
||||
!if "$(MODELREF_TARGET_TYPE)" == "NONE"
|
||||
#--- Stand-alone model ---
|
||||
$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS)
|
||||
@cmd /C "echo ### Linking ..."
|
||||
$(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
|
||||
$(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@
|
||||
@del $(CMD_FILE)
|
||||
@cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe"
|
||||
!else
|
||||
#--- Model reference code generation Target ---
|
||||
$(PRODUCT) : $(OBJS) $(SHARED_LIB)
|
||||
@cmd /C "echo ### Linking ..."
|
||||
$(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
|
||||
$(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB)
|
||||
@cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)"
|
||||
!endif
|
||||
|
||||
{$(MATLAB_ROOT)\rtw\c\src}.c.obj :
|
||||
@cmd /C "echo ### Compiling $<"
|
||||
$(CC) $(CFLAGS) $<
|
||||
|
||||
{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj :
|
||||
@cmd /C "echo ### Compiling $<"
|
||||
$(CC) $(CFLAGS) $<
|
||||
|
||||
{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj :
|
||||
@cmd /C "echo ### Compiling $<"
|
||||
$(CC) $(CFLAGS) $<
|
||||
|
||||
{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj :
|
||||
@cmd /C "echo ### Compiling $<"
|
||||
$(CC) $(CFLAGS) $<
|
||||
|
||||
{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj :
|
||||
@cmd /C "echo ### Compiling $<"
|
||||
$(CC) $(CFLAGS) $<
|
||||
|
||||
# Additional sources
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# Put these rule last, otherwise nmake will check toolboxes first
|
||||
|
||||
{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj :
|
||||
@cmd /C "echo ### Compiling $<"
|
||||
$(CC) $(CFLAGS) $<
|
||||
|
||||
{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj :
|
||||
@cmd /C "echo ### Compiling $<"
|
||||
$(CC) $(CPPFLAGS) $<
|
||||
|
||||
.c.obj :
|
||||
@cmd /C "echo ### Compiling $<"
|
||||
$(CC) $(CFLAGS) $<
|
||||
|
||||
.cpp.obj :
|
||||
@cmd /C "echo ### Compiling $<"
|
||||
$(CC) $(CPPFLAGS) $<
|
||||
|
||||
!if "$(SHARED_LIB)" != ""
|
||||
$(SHARED_LIB) : $(SHARED_SRC)
|
||||
@cmd /C "echo ### Creating $@"
|
||||
@$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<<
|
||||
$?
|
||||
<<
|
||||
@$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS)
|
||||
@cmd /C "echo ### $@ Created"
|
||||
!endif
|
||||
|
||||
|
||||
set_environment_variables:
|
||||
@set INCLUDE=$(INCLUDE)
|
||||
@set LIB=$(LIB)
|
||||
|
||||
# Libraries:
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#----------------------------- Dependencies -----------------------------------
|
||||
|
||||
$(OBJS) : $(MAKEFILE) rtw_proj.tmw
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_terminate'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_terminate'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -14,6 +14,8 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:22 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
Binary file not shown.
Executable
+37
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
* cross.c
|
||||
*
|
||||
* Code generation for function 'cross'
|
||||
*
|
||||
* C source code generated on: Mon Sep 17 20:13:23 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "cross.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
|
||||
{
|
||||
c[0] = a[1] * b[2] - a[2] * b[1];
|
||||
c[1] = a[2] * b[0] - a[0] * b[2];
|
||||
c[2] = a[0] * b[1] - a[1] * b[0];
|
||||
}
|
||||
|
||||
/* End of code generation (cross.c) */
|
||||
Executable
+34
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* cross.h
|
||||
*
|
||||
* Code generation for function 'cross'
|
||||
*
|
||||
* C source code generated on: Mon Sep 17 20:13:23 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __CROSS_H__
|
||||
#define __CROSS_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
|
||||
#endif
|
||||
/* End of code generation (cross.h) */
|
||||
Executable
+42
@@ -0,0 +1,42 @@
|
||||
/*
|
||||
* diag.c
|
||||
*
|
||||
* Code generation for function 'diag'
|
||||
*
|
||||
* C source code generated on: Mon Sep 17 20:13:22 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "diag.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void diag(const real32_T v[3], real32_T d[9])
|
||||
{
|
||||
int32_T j;
|
||||
for (j = 0; j < 9; j++) {
|
||||
d[j] = 0.0F;
|
||||
}
|
||||
|
||||
for (j = 0; j < 3; j++) {
|
||||
d[j + 3 * j] = v[j];
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (diag.c) */
|
||||
Executable
+34
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* diag.h
|
||||
*
|
||||
* Code generation for function 'diag'
|
||||
*
|
||||
* C source code generated on: Mon Sep 17 20:13:23 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __DIAG_H__
|
||||
#define __DIAG_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void diag(const real32_T v[3], real32_T d[9]);
|
||||
#endif
|
||||
/* End of code generation (diag.h) */
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'eye'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:23 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -27,12 +27,12 @@
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void b_eye(real_T I[144])
|
||||
void b_eye(real_T I[81])
|
||||
{
|
||||
int32_T i;
|
||||
memset((void *)&I[0], 0, 144U * sizeof(real_T));
|
||||
for (i = 0; i < 12; i++) {
|
||||
I[i + 12 * i] = 1.0;
|
||||
memset((void *)&I[0], 0, 81U * sizeof(real_T));
|
||||
for (i = 0; i < 9; i++) {
|
||||
I[i + 9 * i] = 1.0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'eye'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:23 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -14,6 +14,8 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
@@ -27,7 +29,7 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void b_eye(real_T I[144]);
|
||||
extern void b_eye(real_T I[81]);
|
||||
extern void eye(real_T I[9]);
|
||||
#endif
|
||||
/* End of code generation (eye.h) */
|
||||
|
||||
Executable
+77
@@ -0,0 +1,77 @@
|
||||
/*
|
||||
* find.c
|
||||
*
|
||||
* Code generation for function 'find'
|
||||
*
|
||||
* C source code generated on: Mon Sep 17 20:13:22 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "find.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1])
|
||||
{
|
||||
int32_T idx;
|
||||
int32_T ii;
|
||||
boolean_T exitg1;
|
||||
boolean_T guard1 = FALSE;
|
||||
int32_T i2;
|
||||
int8_T b_i_data[9];
|
||||
idx = 0;
|
||||
i_sizes[0] = 9;
|
||||
ii = 1;
|
||||
exitg1 = 0U;
|
||||
while ((exitg1 == 0U) && (ii <= 9)) {
|
||||
guard1 = FALSE;
|
||||
if (x[ii - 1] != 0) {
|
||||
idx++;
|
||||
i_data[idx - 1] = (real_T)ii;
|
||||
if (idx >= 9) {
|
||||
exitg1 = 1U;
|
||||
} else {
|
||||
guard1 = TRUE;
|
||||
}
|
||||
} else {
|
||||
guard1 = TRUE;
|
||||
}
|
||||
|
||||
if (guard1 == TRUE) {
|
||||
ii++;
|
||||
}
|
||||
}
|
||||
|
||||
if (1 > idx) {
|
||||
idx = 0;
|
||||
}
|
||||
|
||||
ii = idx - 1;
|
||||
for (i2 = 0; i2 <= ii; i2++) {
|
||||
b_i_data[i2] = (int8_T)i_data[i2];
|
||||
}
|
||||
|
||||
i_sizes[0] = idx;
|
||||
ii = idx - 1;
|
||||
for (i2 = 0; i2 <= ii; i2++) {
|
||||
i_data[i2] = (real_T)b_i_data[i2];
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (find.c) */
|
||||
Executable
+34
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* find.h
|
||||
*
|
||||
* Code generation for function 'find'
|
||||
*
|
||||
* C source code generated on: Mon Sep 17 20:13:22 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __FIND_H__
|
||||
#define __FIND_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]);
|
||||
#endif
|
||||
/* End of code generation (find.h) */
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'mrdivide'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:23 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -21,136 +21,719 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x);
|
||||
static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2],
|
||||
real32_T B_data[81], int32_T B_sizes[2]);
|
||||
static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data
|
||||
[81], int32_T x_sizes[2], int32_T ix0);
|
||||
static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2],
|
||||
real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes
|
||||
[2]);
|
||||
static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T
|
||||
x_sizes[2], int32_T ix0);
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
|
||||
static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x)
|
||||
{
|
||||
int32_T jy;
|
||||
return 0.0F;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2],
|
||||
real32_T B_data[81], int32_T B_sizes[2])
|
||||
{
|
||||
int32_T loop_ub;
|
||||
int32_T iy;
|
||||
real32_T b_A[81];
|
||||
int8_T ipiv[9];
|
||||
real32_T b_A_data[81];
|
||||
int32_T jA;
|
||||
int32_T tmp_data[9];
|
||||
int32_T ipiv_data[9];
|
||||
int32_T ldap1;
|
||||
int32_T j;
|
||||
int32_T mmj;
|
||||
int32_T jj;
|
||||
int32_T jp1j;
|
||||
int32_T c;
|
||||
int32_T ix;
|
||||
real32_T temp;
|
||||
int32_T k;
|
||||
real32_T s;
|
||||
int32_T loop_ub;
|
||||
real32_T Y[108];
|
||||
for (jy = 0; jy < 9; jy++) {
|
||||
for (iy = 0; iy < 9; iy++) {
|
||||
b_A[iy + 9 * jy] = B[jy + 9 * iy];
|
||||
}
|
||||
|
||||
ipiv[jy] = (int8_T)(1 + jy);
|
||||
int32_T jrow;
|
||||
int32_T b_loop_ub;
|
||||
int32_T c;
|
||||
loop_ub = A_sizes[0] * A_sizes[1] - 1;
|
||||
for (iy = 0; iy <= loop_ub; iy++) {
|
||||
b_A_data[iy] = A_data[iy];
|
||||
}
|
||||
|
||||
for (j = 0; j < 8; j++) {
|
||||
mmj = -j;
|
||||
jj = j * 10;
|
||||
jp1j = jj + 1;
|
||||
c = mmj + 9;
|
||||
jy = 0;
|
||||
ix = jj;
|
||||
temp = fabsf(b_A[jj]);
|
||||
for (k = 2; k <= c; k++) {
|
||||
ix++;
|
||||
s = fabsf(b_A[ix]);
|
||||
if (s > temp) {
|
||||
jy = k - 1;
|
||||
temp = s;
|
||||
iy = A_sizes[1];
|
||||
jA = A_sizes[1];
|
||||
jA = iy <= jA ? iy : jA;
|
||||
if (jA < 1) {
|
||||
} else {
|
||||
loop_ub = jA - 1;
|
||||
for (iy = 0; iy <= loop_ub; iy++) {
|
||||
tmp_data[iy] = 1 + iy;
|
||||
}
|
||||
|
||||
loop_ub = jA - 1;
|
||||
for (iy = 0; iy <= loop_ub; iy++) {
|
||||
ipiv_data[iy] = tmp_data[iy];
|
||||
}
|
||||
}
|
||||
|
||||
ldap1 = A_sizes[1] + 1;
|
||||
iy = A_sizes[1] - 1;
|
||||
jA = A_sizes[1];
|
||||
loop_ub = iy <= jA ? iy : jA;
|
||||
for (j = 1; j <= loop_ub; j++) {
|
||||
mmj = (A_sizes[1] - j) + 1;
|
||||
jj = (j - 1) * ldap1;
|
||||
if (mmj < 1) {
|
||||
jA = -1;
|
||||
} else {
|
||||
jA = 0;
|
||||
if (mmj > 1) {
|
||||
ix = jj;
|
||||
temp = (real32_T)fabs(b_A_data[jj]);
|
||||
for (k = 1; k + 1 <= mmj; k++) {
|
||||
ix++;
|
||||
s = (real32_T)fabs(b_A_data[ix]);
|
||||
if (s > temp) {
|
||||
jA = k;
|
||||
temp = s;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ((real_T)b_A[jj + jy] != 0.0) {
|
||||
if (jy != 0) {
|
||||
ipiv[j] = (int8_T)((j + jy) + 1);
|
||||
ix = j;
|
||||
iy = j + jy;
|
||||
for (k = 0; k < 9; k++) {
|
||||
temp = b_A[ix];
|
||||
b_A[ix] = b_A[iy];
|
||||
b_A[iy] = temp;
|
||||
ix += 9;
|
||||
iy += 9;
|
||||
if ((real_T)b_A_data[jj + jA] != 0.0) {
|
||||
if (jA != 0) {
|
||||
ipiv_data[j - 1] = j + jA;
|
||||
jrow = j - 1;
|
||||
iy = jrow + jA;
|
||||
for (k = 1; k <= A_sizes[1]; k++) {
|
||||
temp = b_A_data[jrow];
|
||||
b_A_data[jrow] = b_A_data[iy];
|
||||
b_A_data[iy] = temp;
|
||||
jrow += A_sizes[1];
|
||||
iy += A_sizes[1];
|
||||
}
|
||||
}
|
||||
|
||||
loop_ub = (jp1j + mmj) + 8;
|
||||
for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
|
||||
b_A[iy] /= b_A[jj];
|
||||
b_loop_ub = jj + mmj;
|
||||
for (jA = jj + 1; jA + 1 <= b_loop_ub; jA++) {
|
||||
b_A_data[jA] /= b_A_data[jj];
|
||||
}
|
||||
}
|
||||
|
||||
c = 8 - j;
|
||||
jy = jj + 9;
|
||||
for (iy = 1; iy <= c; iy++) {
|
||||
if ((real_T)b_A[jy] != 0.0) {
|
||||
temp = b_A[jy] * -1.0F;
|
||||
ix = jp1j;
|
||||
loop_ub = (mmj + jj) + 18;
|
||||
for (k = 10 + jj; k + 1 <= loop_ub; k++) {
|
||||
b_A[k] += b_A[ix] * temp;
|
||||
c = A_sizes[1] - j;
|
||||
jA = jj + ldap1;
|
||||
iy = jj + A_sizes[1];
|
||||
for (jrow = 1; jrow <= c; jrow++) {
|
||||
if ((real_T)b_A_data[iy] != 0.0) {
|
||||
temp = b_A_data[iy] * -1.0F;
|
||||
ix = jj;
|
||||
b_loop_ub = (mmj + jA) - 1;
|
||||
for (k = jA; k + 1 <= b_loop_ub; k++) {
|
||||
b_A_data[k] += b_A_data[ix + 1] * temp;
|
||||
ix++;
|
||||
}
|
||||
}
|
||||
|
||||
jy += 9;
|
||||
jj += 9;
|
||||
iy += A_sizes[1];
|
||||
jA += A_sizes[1];
|
||||
}
|
||||
}
|
||||
|
||||
for (jy = 0; jy < 12; jy++) {
|
||||
for (iy = 0; iy < 9; iy++) {
|
||||
Y[iy + 9 * jy] = A[jy + 12 * iy];
|
||||
}
|
||||
}
|
||||
|
||||
for (iy = 0; iy < 9; iy++) {
|
||||
if (ipiv[iy] != iy + 1) {
|
||||
for (j = 0; j < 12; j++) {
|
||||
temp = Y[iy + 9 * j];
|
||||
Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1];
|
||||
Y[(ipiv[iy] + 9 * j) - 1] = temp;
|
||||
for (jA = 0; jA + 1 <= A_sizes[1]; jA++) {
|
||||
if (ipiv_data[jA] != jA + 1) {
|
||||
for (j = 0; j < 9; j++) {
|
||||
temp = B_data[jA + B_sizes[0] * j];
|
||||
B_data[jA + B_sizes[0] * j] = B_data[(ipiv_data[jA] + B_sizes[0] * j) -
|
||||
1];
|
||||
B_data[(ipiv_data[jA] + B_sizes[0] * j) - 1] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (j = 0; j < 12; j++) {
|
||||
c = 9 * j;
|
||||
for (k = 0; k < 9; k++) {
|
||||
jy = 9 * k;
|
||||
if ((real_T)Y[k + c] != 0.0) {
|
||||
for (iy = k + 2; iy < 10; iy++) {
|
||||
Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
|
||||
if (B_sizes[0] == 0) {
|
||||
} else {
|
||||
for (j = 0; j < 9; j++) {
|
||||
c = A_sizes[1] * j;
|
||||
for (k = 0; k + 1 <= A_sizes[1]; k++) {
|
||||
iy = A_sizes[1] * k;
|
||||
if ((real_T)B_data[k + c] != 0.0) {
|
||||
for (jA = k + 1; jA + 1 <= A_sizes[1]; jA++) {
|
||||
B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (j = 0; j < 12; j++) {
|
||||
c = 9 * j;
|
||||
for (k = 8; k > -1; k += -1) {
|
||||
jy = 9 * k;
|
||||
if ((real_T)Y[k + c] != 0.0) {
|
||||
Y[k + c] /= b_A[k + jy];
|
||||
for (iy = 0; iy + 1 <= k; iy++) {
|
||||
Y[iy + c] -= Y[k + c] * b_A[iy + jy];
|
||||
if (B_sizes[0] == 0) {
|
||||
} else {
|
||||
for (j = 0; j < 9; j++) {
|
||||
c = A_sizes[1] * j;
|
||||
for (k = A_sizes[1] - 1; k + 1 > 0; k--) {
|
||||
iy = A_sizes[1] * k;
|
||||
if ((real_T)B_data[k + c] != 0.0) {
|
||||
B_data[k + c] /= b_A_data[k + iy];
|
||||
for (jA = 0; jA + 1 <= k; jA++) {
|
||||
B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data
|
||||
[81], int32_T x_sizes[2], int32_T ix0)
|
||||
{
|
||||
real32_T tau;
|
||||
int32_T nm1;
|
||||
real32_T xnorm;
|
||||
real32_T a;
|
||||
int32_T knt;
|
||||
int32_T loop_ub;
|
||||
int32_T k;
|
||||
tau = 0.0F;
|
||||
if (n <= 0) {
|
||||
} else {
|
||||
nm1 = n - 2;
|
||||
xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0);
|
||||
if ((real_T)xnorm != 0.0) {
|
||||
a = (real32_T)fabs(*alpha1);
|
||||
xnorm = (real32_T)fabs(xnorm);
|
||||
if (a < xnorm) {
|
||||
a /= xnorm;
|
||||
xnorm *= (real32_T)sqrt(a * a + 1.0F);
|
||||
} else if (a > xnorm) {
|
||||
xnorm /= a;
|
||||
xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a;
|
||||
} else if (rtIsNaNF(xnorm)) {
|
||||
} else {
|
||||
xnorm = a * 1.41421354F;
|
||||
}
|
||||
|
||||
if ((real_T)*alpha1 >= 0.0) {
|
||||
xnorm = -xnorm;
|
||||
}
|
||||
|
||||
if ((real32_T)fabs(xnorm) < 9.86076132E-32F) {
|
||||
knt = 0;
|
||||
do {
|
||||
knt++;
|
||||
loop_ub = ix0 + nm1;
|
||||
for (k = ix0; k <= loop_ub; k++) {
|
||||
x_data[k - 1] *= 1.01412048E+31F;
|
||||
}
|
||||
|
||||
xnorm *= 1.01412048E+31F;
|
||||
*alpha1 *= 1.01412048E+31F;
|
||||
} while (!((real32_T)fabs(xnorm) >= 9.86076132E-32F));
|
||||
|
||||
xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0);
|
||||
a = (real32_T)fabs(*alpha1);
|
||||
xnorm = (real32_T)fabs(xnorm);
|
||||
if (a < xnorm) {
|
||||
a /= xnorm;
|
||||
xnorm *= (real32_T)sqrt(a * a + 1.0F);
|
||||
} else if (a > xnorm) {
|
||||
xnorm /= a;
|
||||
xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a;
|
||||
} else if (rtIsNaNF(xnorm)) {
|
||||
} else {
|
||||
xnorm = a * 1.41421354F;
|
||||
}
|
||||
|
||||
if ((real_T)*alpha1 >= 0.0) {
|
||||
xnorm = -xnorm;
|
||||
}
|
||||
|
||||
tau = (xnorm - *alpha1) / xnorm;
|
||||
*alpha1 = 1.0F / (*alpha1 - xnorm);
|
||||
loop_ub = ix0 + nm1;
|
||||
for (k = ix0; k <= loop_ub; k++) {
|
||||
x_data[k - 1] *= *alpha1;
|
||||
}
|
||||
|
||||
for (k = 1; k <= knt; k++) {
|
||||
xnorm *= 9.86076132E-32F;
|
||||
}
|
||||
|
||||
*alpha1 = xnorm;
|
||||
} else {
|
||||
tau = (xnorm - *alpha1) / xnorm;
|
||||
*alpha1 = 1.0F / (*alpha1 - xnorm);
|
||||
loop_ub = ix0 + nm1;
|
||||
for (k = ix0; k <= loop_ub; k++) {
|
||||
x_data[k - 1] *= *alpha1;
|
||||
}
|
||||
|
||||
*alpha1 = xnorm;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return tau;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2],
|
||||
real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes
|
||||
[2])
|
||||
{
|
||||
real_T rankR;
|
||||
real_T u1;
|
||||
int32_T mn;
|
||||
int32_T b_A_sizes[2];
|
||||
int32_T loop_ub;
|
||||
int32_T k;
|
||||
real32_T b_A_data[81];
|
||||
int32_T pvt;
|
||||
int32_T b_mn;
|
||||
int32_T tmp_data[9];
|
||||
int32_T jpvt_data[9];
|
||||
int8_T unnamed_idx_0;
|
||||
real32_T work_data[9];
|
||||
int32_T nmi;
|
||||
real32_T vn1_data[9];
|
||||
real32_T vn2_data[9];
|
||||
int32_T i;
|
||||
int32_T i_i;
|
||||
int32_T mmi;
|
||||
int32_T ix;
|
||||
real32_T smax;
|
||||
real32_T s;
|
||||
int32_T iy;
|
||||
real32_T atmp;
|
||||
real32_T tau_data[9];
|
||||
int32_T i_ip1;
|
||||
int32_T lastv;
|
||||
int32_T lastc;
|
||||
boolean_T exitg2;
|
||||
int32_T exitg1;
|
||||
boolean_T firstNonZero;
|
||||
real32_T t;
|
||||
rankR = (real_T)A_sizes[0];
|
||||
u1 = (real_T)A_sizes[1];
|
||||
rankR = rankR <= u1 ? rankR : u1;
|
||||
mn = (int32_T)rankR;
|
||||
b_A_sizes[0] = A_sizes[0];
|
||||
b_A_sizes[1] = A_sizes[1];
|
||||
loop_ub = A_sizes[0] * A_sizes[1] - 1;
|
||||
for (k = 0; k <= loop_ub; k++) {
|
||||
b_A_data[k] = A_data[k];
|
||||
}
|
||||
|
||||
k = A_sizes[0];
|
||||
pvt = A_sizes[1];
|
||||
b_mn = k <= pvt ? k : pvt;
|
||||
if (A_sizes[1] < 1) {
|
||||
} else {
|
||||
loop_ub = A_sizes[1] - 1;
|
||||
for (k = 0; k <= loop_ub; k++) {
|
||||
tmp_data[k] = 1 + k;
|
||||
}
|
||||
|
||||
loop_ub = A_sizes[1] - 1;
|
||||
for (k = 0; k <= loop_ub; k++) {
|
||||
jpvt_data[k] = tmp_data[k];
|
||||
}
|
||||
}
|
||||
|
||||
if ((A_sizes[0] == 0) || (A_sizes[1] == 0)) {
|
||||
} else {
|
||||
unnamed_idx_0 = (int8_T)A_sizes[1];
|
||||
loop_ub = unnamed_idx_0 - 1;
|
||||
for (k = 0; k <= loop_ub; k++) {
|
||||
work_data[k] = 0.0F;
|
||||
}
|
||||
|
||||
k = 1;
|
||||
for (nmi = 0; nmi + 1 <= A_sizes[1]; nmi++) {
|
||||
vn1_data[nmi] = eml_xnrm2(A_sizes[0], A_data, A_sizes, k);
|
||||
vn2_data[nmi] = vn1_data[nmi];
|
||||
k += A_sizes[0];
|
||||
}
|
||||
|
||||
for (i = 0; i + 1 <= b_mn; i++) {
|
||||
i_i = i + i * A_sizes[0];
|
||||
nmi = A_sizes[1] - i;
|
||||
mmi = (A_sizes[0] - i) - 1;
|
||||
if (nmi < 1) {
|
||||
pvt = -1;
|
||||
} else {
|
||||
pvt = 0;
|
||||
if (nmi > 1) {
|
||||
ix = i;
|
||||
smax = (real32_T)fabs(vn1_data[i]);
|
||||
for (k = 1; k + 1 <= nmi; k++) {
|
||||
ix++;
|
||||
s = (real32_T)fabs(vn1_data[ix]);
|
||||
if (s > smax) {
|
||||
pvt = k;
|
||||
smax = s;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pvt += i;
|
||||
if (pvt + 1 != i + 1) {
|
||||
ix = A_sizes[0] * pvt;
|
||||
iy = A_sizes[0] * i;
|
||||
for (k = 1; k <= A_sizes[0]; k++) {
|
||||
s = b_A_data[ix];
|
||||
b_A_data[ix] = b_A_data[iy];
|
||||
b_A_data[iy] = s;
|
||||
ix++;
|
||||
iy++;
|
||||
}
|
||||
|
||||
k = jpvt_data[pvt];
|
||||
jpvt_data[pvt] = jpvt_data[i];
|
||||
jpvt_data[i] = k;
|
||||
vn1_data[pvt] = vn1_data[i];
|
||||
vn2_data[pvt] = vn2_data[i];
|
||||
}
|
||||
|
||||
if (i + 1 < A_sizes[0]) {
|
||||
atmp = b_A_data[i_i];
|
||||
smax = eml_matlab_zlarfg(mmi + 1, &atmp, b_A_data, b_A_sizes, i_i + 2);
|
||||
tau_data[i] = smax;
|
||||
} else {
|
||||
atmp = b_A_data[i_i];
|
||||
smax = b_A_data[i_i];
|
||||
s = b_eml_matlab_zlarfg(&atmp, &smax);
|
||||
b_A_data[i_i] = smax;
|
||||
tau_data[i] = s;
|
||||
}
|
||||
|
||||
b_A_data[i_i] = atmp;
|
||||
if (i + 1 < A_sizes[1]) {
|
||||
atmp = b_A_data[i_i];
|
||||
b_A_data[i_i] = 1.0F;
|
||||
i_ip1 = (i + (i + 1) * A_sizes[0]) + 1;
|
||||
if ((real_T)tau_data[i] != 0.0) {
|
||||
lastv = mmi;
|
||||
pvt = i_i + mmi;
|
||||
while ((lastv + 1 > 0) && ((real_T)b_A_data[pvt] == 0.0)) {
|
||||
lastv--;
|
||||
pvt--;
|
||||
}
|
||||
|
||||
lastc = nmi - 1;
|
||||
exitg2 = 0U;
|
||||
while ((exitg2 == 0U) && (lastc > 0)) {
|
||||
k = i_ip1 + (lastc - 1) * A_sizes[0];
|
||||
pvt = k + lastv;
|
||||
do {
|
||||
exitg1 = 0U;
|
||||
if (k <= pvt) {
|
||||
if ((real_T)b_A_data[k - 1] != 0.0) {
|
||||
exitg1 = 1U;
|
||||
} else {
|
||||
k++;
|
||||
}
|
||||
} else {
|
||||
lastc--;
|
||||
exitg1 = 2U;
|
||||
}
|
||||
} while (exitg1 == 0U);
|
||||
|
||||
if (exitg1 == 1U) {
|
||||
exitg2 = 1U;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
lastv = -1;
|
||||
lastc = 0;
|
||||
}
|
||||
|
||||
if (lastv + 1 > 0) {
|
||||
if (lastc == 0) {
|
||||
} else {
|
||||
for (iy = 1; iy <= lastc; iy++) {
|
||||
work_data[iy - 1] = 0.0F;
|
||||
}
|
||||
|
||||
iy = 0;
|
||||
loop_ub = i_ip1 + A_sizes[0] * (lastc - 1);
|
||||
pvt = i_ip1;
|
||||
while ((A_sizes[0] > 0) && (pvt <= loop_ub)) {
|
||||
ix = i_i;
|
||||
smax = 0.0F;
|
||||
k = pvt + lastv;
|
||||
for (nmi = pvt; nmi <= k; nmi++) {
|
||||
smax += b_A_data[nmi - 1] * b_A_data[ix];
|
||||
ix++;
|
||||
}
|
||||
|
||||
work_data[iy] += smax;
|
||||
iy++;
|
||||
pvt += A_sizes[0];
|
||||
}
|
||||
}
|
||||
|
||||
smax = -tau_data[i];
|
||||
if ((real_T)smax == 0.0) {
|
||||
} else {
|
||||
pvt = 0;
|
||||
for (nmi = 1; nmi <= lastc; nmi++) {
|
||||
if ((real_T)work_data[pvt] != 0.0) {
|
||||
s = work_data[pvt] * smax;
|
||||
ix = i_i;
|
||||
loop_ub = lastv + i_ip1;
|
||||
for (k = i_ip1 - 1; k + 1 <= loop_ub; k++) {
|
||||
b_A_data[k] += b_A_data[ix] * s;
|
||||
ix++;
|
||||
}
|
||||
}
|
||||
|
||||
pvt++;
|
||||
i_ip1 += A_sizes[0];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
b_A_data[i_i] = atmp;
|
||||
}
|
||||
|
||||
for (nmi = i + 1; nmi + 1 <= A_sizes[1]; nmi++) {
|
||||
if ((real_T)vn1_data[nmi] != 0.0) {
|
||||
smax = (real32_T)fabs(b_A_data[i + b_A_sizes[0] * nmi]) / vn1_data[nmi];
|
||||
smax = 1.0F - smax * smax;
|
||||
if ((real_T)smax < 0.0) {
|
||||
smax = 0.0F;
|
||||
}
|
||||
|
||||
s = vn1_data[nmi] / vn2_data[nmi];
|
||||
if (smax * (s * s) <= 0.000345266977F) {
|
||||
if (i + 1 < A_sizes[0]) {
|
||||
k = (i + A_sizes[0] * nmi) + 1;
|
||||
smax = 0.0F;
|
||||
if (mmi < 1) {
|
||||
} else if (mmi == 1) {
|
||||
smax = (real32_T)fabs(b_A_data[k]);
|
||||
} else {
|
||||
s = 0.0F;
|
||||
firstNonZero = TRUE;
|
||||
pvt = k + mmi;
|
||||
while (k + 1 <= pvt) {
|
||||
if (b_A_data[k] != 0.0F) {
|
||||
atmp = (real32_T)fabs(b_A_data[k]);
|
||||
if (firstNonZero) {
|
||||
s = atmp;
|
||||
smax = 1.0F;
|
||||
firstNonZero = FALSE;
|
||||
} else if (s < atmp) {
|
||||
t = s / atmp;
|
||||
smax = 1.0F + smax * t * t;
|
||||
s = atmp;
|
||||
} else {
|
||||
t = atmp / s;
|
||||
smax += t * t;
|
||||
}
|
||||
}
|
||||
|
||||
k++;
|
||||
}
|
||||
|
||||
smax = s * (real32_T)sqrt(smax);
|
||||
}
|
||||
|
||||
vn1_data[nmi] = smax;
|
||||
vn2_data[nmi] = vn1_data[nmi];
|
||||
} else {
|
||||
vn1_data[nmi] = 0.0F;
|
||||
vn2_data[nmi] = 0.0F;
|
||||
}
|
||||
} else {
|
||||
vn1_data[nmi] *= (real32_T)sqrt(smax);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (jy = 0; jy < 9; jy++) {
|
||||
for (iy = 0; iy < 12; iy++) {
|
||||
y[iy + 12 * jy] = Y[jy + 9 * iy];
|
||||
rankR = (real_T)A_sizes[0];
|
||||
u1 = (real_T)A_sizes[1];
|
||||
rankR = rankR >= u1 ? rankR : u1;
|
||||
smax = (real32_T)rankR * (real32_T)fabs(b_A_data[0]) * 1.1920929E-7F;
|
||||
rankR = 0.0;
|
||||
k = 0;
|
||||
while ((k + 1 <= mn) && (!((real32_T)fabs(b_A_data[k + b_A_sizes[0] * k]) <=
|
||||
smax))) {
|
||||
rankR++;
|
||||
k++;
|
||||
}
|
||||
|
||||
unnamed_idx_0 = (int8_T)A_sizes[1];
|
||||
Y_sizes[0] = (int32_T)unnamed_idx_0;
|
||||
Y_sizes[1] = 9;
|
||||
loop_ub = unnamed_idx_0 * 9 - 1;
|
||||
for (k = 0; k <= loop_ub; k++) {
|
||||
Y_data[k] = 0.0F;
|
||||
}
|
||||
|
||||
for (nmi = 0; nmi + 1 <= mn; nmi++) {
|
||||
if ((real_T)tau_data[nmi] != 0.0) {
|
||||
for (k = 0; k < 9; k++) {
|
||||
smax = B_data[nmi + B_sizes[0] * k];
|
||||
for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) {
|
||||
smax += b_A_data[i + b_A_sizes[0] * nmi] * B_data[i + B_sizes[0] * k];
|
||||
}
|
||||
|
||||
smax *= tau_data[nmi];
|
||||
if ((real_T)smax != 0.0) {
|
||||
B_data[nmi + B_sizes[0] * k] -= smax;
|
||||
for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) {
|
||||
B_data[i + B_sizes[0] * k] -= b_A_data[i + b_A_sizes[0] * nmi] *
|
||||
smax;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (k = 0; k < 9; k++) {
|
||||
for (i = 0; (real_T)(i + 1) <= rankR; i++) {
|
||||
Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] = B_data[i + B_sizes[0] * k];
|
||||
}
|
||||
|
||||
for (nmi = (int32_T)rankR - 1; nmi + 1 >= 1; nmi--) {
|
||||
Y_data[(jpvt_data[nmi] + Y_sizes[0] * k) - 1] /= b_A_data[nmi + b_A_sizes
|
||||
[0] * nmi];
|
||||
for (i = 0; i + 1 <= nmi; i++) {
|
||||
Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] -= Y_data[(jpvt_data[nmi] +
|
||||
Y_sizes[0] * k) - 1] * b_A_data[i + b_A_sizes[0] * nmi];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T
|
||||
x_sizes[2], int32_T ix0)
|
||||
{
|
||||
real32_T y;
|
||||
real32_T scale;
|
||||
boolean_T firstNonZero;
|
||||
int32_T kend;
|
||||
int32_T k;
|
||||
real32_T absxk;
|
||||
real32_T t;
|
||||
y = 0.0F;
|
||||
if (n < 1) {
|
||||
} else if (n == 1) {
|
||||
y = (real32_T)fabs(x_data[ix0 - 1]);
|
||||
} else {
|
||||
scale = 0.0F;
|
||||
firstNonZero = TRUE;
|
||||
kend = (ix0 + n) - 1;
|
||||
for (k = ix0 - 1; k + 1 <= kend; k++) {
|
||||
if (x_data[k] != 0.0F) {
|
||||
absxk = (real32_T)fabs(x_data[k]);
|
||||
if (firstNonZero) {
|
||||
scale = absxk;
|
||||
y = 1.0F;
|
||||
firstNonZero = FALSE;
|
||||
} else if (scale < absxk) {
|
||||
t = scale / absxk;
|
||||
y = 1.0F + y * t * t;
|
||||
scale = absxk;
|
||||
} else {
|
||||
t = absxk / scale;
|
||||
y += t * t;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
y = scale * (real32_T)sqrt(y);
|
||||
}
|
||||
|
||||
return y;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const
|
||||
real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81],
|
||||
int32_T y_sizes[2])
|
||||
{
|
||||
int32_T b_A_sizes[2];
|
||||
int32_T loop_ub;
|
||||
int32_T i3;
|
||||
int32_T b_loop_ub;
|
||||
int32_T i4;
|
||||
real32_T b_A_data[81];
|
||||
int32_T b_B_sizes[2];
|
||||
real32_T b_B_data[81];
|
||||
int8_T unnamed_idx_0;
|
||||
int32_T c_B_sizes[2];
|
||||
real32_T c_B_data[81];
|
||||
b_A_sizes[0] = B_sizes[1];
|
||||
b_A_sizes[1] = B_sizes[0];
|
||||
loop_ub = B_sizes[0] - 1;
|
||||
for (i3 = 0; i3 <= loop_ub; i3++) {
|
||||
b_loop_ub = B_sizes[1] - 1;
|
||||
for (i4 = 0; i4 <= b_loop_ub; i4++) {
|
||||
b_A_data[i4 + b_A_sizes[0] * i3] = B_data[i3 + B_sizes[0] * i4];
|
||||
}
|
||||
}
|
||||
|
||||
b_B_sizes[0] = A_sizes[1];
|
||||
b_B_sizes[1] = 9;
|
||||
for (i3 = 0; i3 < 9; i3++) {
|
||||
loop_ub = A_sizes[1] - 1;
|
||||
for (i4 = 0; i4 <= loop_ub; i4++) {
|
||||
b_B_data[i4 + b_B_sizes[0] * i3] = A_data[i3 + A_sizes[0] * i4];
|
||||
}
|
||||
}
|
||||
|
||||
if ((b_A_sizes[0] == 0) || (b_A_sizes[1] == 0) || (b_B_sizes[0] == 0)) {
|
||||
unnamed_idx_0 = (int8_T)b_A_sizes[1];
|
||||
b_B_sizes[0] = (int32_T)unnamed_idx_0;
|
||||
b_B_sizes[1] = 9;
|
||||
loop_ub = unnamed_idx_0 * 9 - 1;
|
||||
for (i3 = 0; i3 <= loop_ub; i3++) {
|
||||
b_B_data[i3] = 0.0F;
|
||||
}
|
||||
} else if (b_A_sizes[0] == b_A_sizes[1]) {
|
||||
eml_lusolve(b_A_data, b_A_sizes, b_B_data, b_B_sizes);
|
||||
} else {
|
||||
c_B_sizes[0] = b_B_sizes[0];
|
||||
c_B_sizes[1] = 9;
|
||||
loop_ub = b_B_sizes[0] * 9 - 1;
|
||||
for (i3 = 0; i3 <= loop_ub; i3++) {
|
||||
c_B_data[i3] = b_B_data[i3];
|
||||
}
|
||||
|
||||
eml_qrsolve(b_A_data, b_A_sizes, c_B_data, c_B_sizes, b_B_data, b_B_sizes);
|
||||
}
|
||||
|
||||
y_sizes[0] = 9;
|
||||
y_sizes[1] = b_B_sizes[0];
|
||||
loop_ub = b_B_sizes[0] - 1;
|
||||
for (i3 = 0; i3 <= loop_ub; i3++) {
|
||||
for (i4 = 0; i4 < 9; i4++) {
|
||||
y_data[i4 + 9 * i3] = b_B_data[i3 + b_B_sizes[0] * i4];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'mrdivide'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:23 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -14,6 +14,8 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
@@ -27,6 +29,6 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
|
||||
extern void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], int32_T y_sizes[2]);
|
||||
#endif
|
||||
/* End of code generation (mrdivide.h) */
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'norm'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:23 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -40,7 +40,7 @@ real32_T norm(const real32_T x[3])
|
||||
firstNonZero = TRUE;
|
||||
for (k = 0; k < 3; k++) {
|
||||
if (x[k] != 0.0F) {
|
||||
absxk = fabsf(x[k]);
|
||||
absxk = (real32_T)fabs(x[k]);
|
||||
if (firstNonZero) {
|
||||
scale = absxk;
|
||||
y = 1.0F;
|
||||
@@ -56,7 +56,7 @@ real32_T norm(const real32_T x[3])
|
||||
}
|
||||
}
|
||||
|
||||
return scale * sqrtf(y);
|
||||
return scale * (real32_T)sqrt(y);
|
||||
}
|
||||
|
||||
/* End of code generation (norm.c) */
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'norm'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:23 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -14,6 +14,8 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "rt_defines.h"
|
||||
#include "rt_nonfinite.h"
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "attitudeKalmanfilter_types.h"
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
+24
@@ -0,0 +1,24 @@
|
||||
/*
|
||||
* rt_defines.h
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RT_DEFINES_H__
|
||||
#define __RT_DEFINES_H__
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#define RT_PI 3.14159265358979323846
|
||||
#define RT_PIF 3.1415927F
|
||||
#define RT_LN_10 2.30258509299404568402
|
||||
#define RT_LN_10F 2.3025851F
|
||||
#define RT_LOG10E 0.43429448190325182765
|
||||
#define RT_LOG10EF 0.43429449F
|
||||
#define RT_E 2.7182818284590452354
|
||||
#define RT_EF 2.7182817F
|
||||
#endif
|
||||
/* End of code generation (rt_defines.h) */
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a.
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Wed Jul 11 08:38:35 2012
|
||||
* C source code generated on: Mon Sep 17 20:13:24 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
Reference in New Issue
Block a user