Merge branch 'master' of github.com:PX4/Firmware

46 files changed, 1329 insertions, 1382 deletions

diff --git a/Makefile b/Makefile
index 43a4333cf..e38a3d619 100644
--- a/Makefile
+++ b/Makefile
@@ -114,6 +114,10 @@ endif
 
 upload:		$(FIRMWARE_BUNDLE) $(UPLOADER)
 	@python -u $(UPLOADER) --port $(SERIAL_PORTS) $(FIRMWARE_BUNDLE)
+	
+upload-jtag-px4fmu:
+	@echo Attempting to flash PX4FMU board via JTAG
+	@openocd -f interface/olimex-jtag-tiny.cfg -f Tools/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
 
 #
 # Hacks and fixups
@@ -129,7 +133,7 @@ endif
 # a complete re-compilation, 'distclean' should remove everything 
 # that's generated leaving only files that are in source control.
 #
-.PHONY:	clean
+.PHONY:	clean upload-jtag-px4fmu
 clean:
 	@make -C $(NUTTX_SRC) -r $(MQUIET) distclean
 	@make -C $(ROMFS_SRC) -r $(MQUIET) clean
diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR
index 2a771cac4..532dd6a25 100644
--- a/ROMFS/scripts/rc.PX4IOAR
+++ b/ROMFS/scripts/rc.PX4IOAR
@@ -2,69 +2,75 @@
 #
 # Flight startup script for PX4FMU on PX4IOAR carrier board.
 #
-
+ 
+set USB no
+ 
 echo "[init] doing PX4IOAR startup..."
-
+ 
 #
 # Start the ORB
 #
 uorb start
-
+ 
+#
+# Init the EEPROM
+#
+echo "[init] eeprom"
+eeprom start
+if [ -f /eeprom/parameters ]
+then
+	eeprom load_param /eeprom/parameters
+fi
+ 
 #
 # Start the sensors.
 #
 sh /etc/init.d/rc.sensors
-
+ 
 #
 # Start MAVLink
 #
 mavlink start -d /dev/ttyS0 -b 57600
 usleep 5000
-
+ 
 #
 # Start the commander.
 #
-# XXX this should be '<command> start'.
-#
-commander &
-
+commander start
+ 
 #
 # Start the attitude estimator
 #
-# XXX this should be '<command> start'.
-#
-attitude_estimator_bm &
-#position_estimator &
-
+attitude_estimator_ekf start
+ 
 #
 # Configure PX4FMU for operation with PX4IOAR
 #
-# XXX arguments?
+fmu mode_gpio_serial
+ 
 #
-#fmu mode_
-
+# Fire up the multi rotor attitude controller
 #
-# Fire up the AR.Drone controller.
+multirotor_att_control start
+ 
 #
-# XXX this should be '<command> start'.
+# Fire up the AR.Drone interface.
 #
-ardrone_control -d /dev/ttyS1 -m attitude &
+ardrone_interface start
 
 #
-# Start looking for a GPS.
-#
-# XXX this should not need to be backgrounded
+# Start logging
 #
-#gps -d /dev/ttyS3 -m all &
+#sdlog start
 
 #
-# Start logging to microSD if we can
+# Start GPS capture
 #
-#sh /etc/init.d/rc.logging
-
+gps start
+ 
 #
 # startup is done; we don't want the shell because we
-# use the same UART for telemetry (dumb).
+# use the same UART for telemetry
 #
-echo "[init] startup done, exiting."
-exit
+echo "[init] startup done"
+exit
\ No newline at end of file
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index 7ebd37e75..296de721b 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -60,6 +60,7 @@ import json
 import zlib
 import base64
 import time
+import array
 
 from sys import platform as _platform
 
@@ -67,7 +68,41 @@ class firmware(object):
 	'''Loads a firmware file'''
 
 	desc = {}
-	image = bytearray()
+	image = bytes()
+	crctab = array.array('I', [
+		0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+		0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+		0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+		0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+		0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+		0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+		0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+		0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+		0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+		0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+		0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+		0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+		0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+		0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+		0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+		0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+		0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+		0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+		0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+		0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+		0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+		0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+		0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+		0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+		0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+		0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+		0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+		0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+		0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+		0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+		0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+		0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d ])
+	crcpad = bytearray('\xff\xff\xff\xff')
 
 	def __init__(self, path):
 
@@ -76,30 +111,48 @@ class firmware(object):
 		self.desc = json.load(f)
 		f.close()
 
-		self.image = zlib.decompress(base64.b64decode(self.desc['image']))
+		self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image'])))
 
 		# pad image to 4-byte length
 		while ((len(self.image) % 4) != 0):
-			self.image += b'\x00'
+			self.image.append('\xff')
 
 	def property(self, propname):
 		return self.desc[propname]
 
+	def __crc32(self, bytes, state):
+		for byte in bytes:
+			index = (state ^ byte) & 0xff
+			state = self.crctab[index] ^ (state >> 8)
+		return state
+
+	def crc(self, padlen):
+		state = self.__crc32(self.image, int(0))
+		for i in range(len(self.image), (padlen - 1), 4):
+			state = self.__crc32(self.crcpad, state)
+		return state
 
 class uploader(object):
 	'''Uploads a firmware file to the PX FMU bootloader'''
 
-	NOP		= chr(0x00)
-	OK		= chr(0x10)
-	FAILED		= chr(0x11)
+	# protocol bytes
 	INSYNC		= chr(0x12)
 	EOC		= chr(0x20)
+
+	# reply bytes
+	OK		= chr(0x10)
+	FAILED		= chr(0x11)
+	INVALID		= chr(0x13)	# rev3+
+
+	# command bytes
+	NOP		= chr(0x00)	# guaranteed to be discarded by the bootloader
 	GET_SYNC	= chr(0x21)
 	GET_DEVICE	= chr(0x22)
 	CHIP_ERASE	= chr(0x23)
-	CHIP_VERIFY	= chr(0x24)
+	CHIP_VERIFY	= chr(0x24)	# rev2 only
 	PROG_MULTI	= chr(0x27)
-	READ_MULTI	= chr(0x28)
+	READ_MULTI	= chr(0x28)	# rev2 only
+	GET_CRC		= chr(0x29)	# rev3+
 	REBOOT		= chr(0x30)
 	
 	INFO_BL_REV	= chr(1)	# bootloader protocol revision
@@ -126,19 +179,28 @@ class uploader(object):
 
 	def __recv(self, count = 1):
 		c = self.port.read(count)
-		if (len(c) < 1):
+		if len(c) < 1:
 			raise RuntimeError("timeout waiting for data")
 #		print("recv " + binascii.hexlify(c))
 		return c
 
+	def __recv_int(self):
+		raw = self.__recv(4)
+		val = struct.unpack("<I", raw)
+		return val[0]
+
 	def __getSync(self):
 		self.port.flush()
 		c = self.__recv()
-		if (c != self.INSYNC):
+		if c is not self.INSYNC:
 			raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c))
 		c = self.__recv()
-		if (c != self.OK):
-			raise RuntimeError("unexpected 0x%x instead of OK" % ord(c))
+		if c == self.INVALID:
+			raise RuntimeError("bootloader reports INVALID OPERATION")
+		if c == self.FAILED:
+			raise RuntimeError("bootloader reports OPERATION FAILED")
+		if c != self.OK:
+			raise RuntimeError("unexpected response 0x%x instead of OK" % ord(c))
 
 	# attempt to get back into sync with the bootloader
 	def __sync(self):
@@ -164,10 +226,9 @@ class uploader(object):
 	# send the GET_DEVICE command and wait for an info parameter
 	def __getInfo(self, param):
 		self.__send(uploader.GET_DEVICE + param + uploader.EOC)
-		raw = self.__recv(4)
+		value = self.__recv_int()
 		self.__getSync()
-		value = struct.unpack_from('<I', raw)
-		return value[0]
+		return value
 
 	# send the CHIP_ERASE command and wait for the bootloader to become ready
 	def __erase(self):
@@ -200,7 +261,7 @@ class uploader(object):
 				+ uploader.EOC)
 		self.port.flush()
 		programmed = self.__recv(len(data))
-		if (programmed != data):
+		if programmed != data:
 			print("got    " + binascii.hexlify(programmed))
 			print("expect " + binascii.hexlify(data))
 			return False
@@ -209,9 +270,14 @@ class uploader(object):
 		
 	# send the reboot command
 	def __reboot(self):
-		self.__send(uploader.REBOOT)
+		self.__send(uploader.REBOOT
+				+ uploader.EOC)
 		self.port.flush()
 
+		# v3+ can report failure if the first word flash fails
+		if self.bl_rev >= 3:
+			self.__getSync()
+
 	# split a sequence into a list of size-constrained pieces
 	def __split_len(self, seq, length):
     		return [seq[i:i+length] for i in range(0, len(seq), length)]
@@ -224,7 +290,7 @@ class uploader(object):
 			self.__program_multi(bytes)
 
 	# verify code
-	def __verify(self, fw):
+	def __verify_v2(self, fw):
 		self.__send(uploader.CHIP_VERIFY
 				+ uploader.EOC)
 		self.__getSync()
@@ -234,14 +300,25 @@ class uploader(object):
 			if (not self.__verify_multi(bytes)):
 				raise RuntimeError("Verification failed")
 
+	def __verify_v3(self, fw):
+		expect_crc = fw.crc(self.fw_maxsize)
+		self.__send(uploader.GET_CRC
+				+ uploader.EOC)
+		report_crc = self.__recv_int()
+		self.__getSync()
+		if report_crc != expect_crc:
+			print("Expected 0x%x" % expect_crc)
+			print("Got      0x%x" % report_crc)
+			raise RuntimeError("Program CRC failed")
+
 	# get basic data about the board
 	def identify(self):
 		# make sure we are in sync before starting
 		self.__sync()
 
 		# get the bootloader protocol ID first
-		bl_rev = self.__getInfo(uploader.INFO_BL_REV)
-		if (bl_rev < uploader.BL_REV_MIN) or (bl_rev > uploader.BL_REV_MAX):
+		self.bl_rev = self.__getInfo(uploader.INFO_BL_REV)
+		if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX):
 			print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV)
 			raise RuntimeError("Bootloader protocol mismatch")
 
@@ -264,7 +341,10 @@ class uploader(object):
 		self.__program(fw)
 
 		print("verify...")
-		self.__verify(fw)
+		if self.bl_rev == 2:
+			self.__verify_v2(fw)
+		else:
+			self.__verify_v3(fw)
 
 		print("done, rebooting.")
 		self.__reboot()
@@ -313,7 +393,7 @@ while True:
 		try:
 			# identify the bootloader
 			up.identify()
-			print("Found board %x,%x on %s" % (up.board_type, up.board_rev, port))
+			print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
 
 		except:
 			# most probably a timeout talking to the port, no bootloader
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index 8ed6db664..74b32c4af 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -56,6 +56,8 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 
+#include <systemlib/systemlib.h>
+
 #include "ardrone_motor_control.h"
 
 __EXPORT int ardrone_interface_main(int argc, char *argv[]);
@@ -112,7 +114,12 @@ int ardrone_interface_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		ardrone_interface_task = task_create("ardrone_interface", SCHED_PRIORITY_MAX - 15, 4096, ardrone_interface_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		ardrone_interface_task = task_spawn("ardrone_interface",
+						    SCHED_RR,
+						    SCHED_PRIORITY_MAX - 15,
+						    4096,
+						    ardrone_interface_thread_main,
+						    (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile
index 932b49f5c..a8f80fd4c 100644
--- a/apps/attitude_estimator_ekf/Makefile
+++ b/apps/attitude_estimator_ekf/Makefile
@@ -45,9 +45,9 @@ CSRCS		 = attitude_estimator_ekf_main.c \
 		   codegen/rtGetInf.c \
 		   codegen/rtGetNaN.c \
 		   codegen/norm.c \
-		   codegen/find.c \
 		   codegen/diag.c \
-		   codegen/cross.c
+		   codegen/cross.c \
+		   codegen/power.c
 
 # XXX this is *horribly* broken
 INCLUDES	+= $(TOPDIR)/../mavlink/include/mavlink
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index 867b484e1..46c1a6623 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -60,6 +60,8 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <arch/board/up_hrt.h>
 
+#include <systemlib/systemlib.h>
+
 #include "codegen/attitudeKalmanfilter_initialize.h"
 #include "codegen/attitudeKalmanfilter.h"
 
@@ -73,38 +75,45 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
 
 static unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
-static float dt = 1;
+static float dt = 1.0f;
 /* 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_k[9] = {0,0,0,0,0,-9.81,1,1,-1};		/**< */
-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,
+static float z_k[9];					/**< Measurement vector */
+static float x_aposteriori_k[12];		/**< */
+static float x_aposteriori[12];
+static float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
+				   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
+				   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
+				   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,
+				   0,   0,   0,   0,  100.f,  0,   0,   0,   0,   0,   0,   0,
+				   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
+				   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
+				   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,
+				   0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
+				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
+				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
+				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
 				  };
-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,
+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.0f, 100.0f,   0,   0,
+				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
+				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
 				  };	/**< init: diagonal matrix with big values */
-static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1};			/**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+// static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 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
 			     };		/**< init: identity matrix */
 
+
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
 static int attitude_estimator_ekf_task;				/**< Handle of deamon task / thread */
@@ -150,7 +159,12 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		attitude_estimator_ekf_task = task_create("attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 20000, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
+							 SCHED_RR,
+							 SCHED_PRIORITY_DEFAULT,
+							 20000,
+							 attitude_estimator_ekf_thread_main,
+							 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
@@ -222,6 +236,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
 	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
 
+
+	/* process noise covariance */
+	float q[12];
+	/* measurement noise covariance */
+	float r[9];
+	/* output euler angles */
+	float euler[3] = {0.0f, 0.0f, 0.0f};
+
 	/* Main loop*/
 	while (!thread_should_exit) {
 
@@ -271,10 +293,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 				overloadcounter++;
 			}
 
-			int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1};
-			float euler[3];
+			uint8_t update_vect[3] = {1, 1, 1};
 			int32_t z_k_sizes = 9;
-			float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+			// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
 
 			static bool const_initialized = false;
 
@@ -282,21 +303,42 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) 
 			{
 				dt = 0.005f;
-				knownConst[0] = 0.6f*0.6f*dt;
-				knownConst[1] = 0.6f*0.6f*dt;
-				knownConst[2] = 0.2f*0.2f*dt;
-				knownConst[3] = 0.2f*0.2f*dt;
-				knownConst[4] = 0.000001f*0.000001f*dt; // -9.81,1,1,-1};
+				q[0] =  1e1f;
+				q[1] =  1e1f;
+				q[2] =  1e1f;
+				/* process noise gyro offset covariance */
+				q[3] =  1e-4f;
+				q[4] =  1e-4f;
+				q[5] =  1e-4f;
+				q[6] =  1e-1f;
+				q[7] =  1e-1f;
+				q[8] =  1e-1f;
+				q[9] =  1e-1f;
+				q[10] = 1e-1f;
+				q[11] = 1e-1f;
+
+				r[0]= 1e-2f;
+				r[1]= 1e-2f;
+				r[2]= 1e-2f;
+				r[3]= 1e-1f;
+				r[4]= 1e-1f;
+				r[5]= 1e-1f;
+				r[6]= 1e-1f;
+				r[7]= 1e-1f;
+				r[8]= 1e-1f;
 
 				x_aposteriori_k[0] = z_k[0];
 				x_aposteriori_k[1] = z_k[1];
 				x_aposteriori_k[2] = z_k[2];
-				x_aposteriori_k[3] = z_k[3];
-				x_aposteriori_k[4] = z_k[4];
-				x_aposteriori_k[5] = z_k[5];
-				x_aposteriori_k[6] = z_k[6];
-				x_aposteriori_k[7] = z_k[7];
-				x_aposteriori_k[8] = z_k[8];
+				x_aposteriori_k[3] = 0.0f;
+				x_aposteriori_k[4] = 0.0f;
+				x_aposteriori_k[5] = 0.0f;
+				x_aposteriori_k[6] = z_k[3];
+				x_aposteriori_k[7] = z_k[4];
+				x_aposteriori_k[8] = z_k[5];
+				x_aposteriori_k[9] = z_k[6];
+				x_aposteriori_k[10] = z_k[7];
+				x_aposteriori_k[11] = z_k[8];
 
 				const_initialized = true;
 			}
@@ -306,39 +348,43 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 				continue;
 			}
 
+			dt = 0.004f;
+
 			uint64_t timing_start = hrt_absolute_time();
-			attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
-				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);
+			attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, q, r,
+				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 % 2 == 0) {
-				// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
-				// 	x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
-				// 	x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
-				// 	x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
+			// /* print rotation matrix every 200th time */
+			// if (printcounter % 200 == 0) {
+			// 	// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
+			// 	// 	x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
+			// 	// 	x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
+			// 	// 	x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
 
 
-				// }
+			// 	// }
 
 			// 	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", (double)euler[0], (double)euler[1], (double)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));
-			}
-
-							int i = printcounter % 9;
-
-				// for (int i = 0; i < 9; i++) {
-					char name[10];
-					sprintf(name, "xapo #%d", i);
-					memcpy(dbg.key, name, sizeof(dbg.key));
-					dbg.value = x_aposteriori[i];
-					orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
+			// 	printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)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));
+			// }
+
+			// 				int i = printcounter % 9;
+
+			// 	// for (int i = 0; i < 9; i++) {
+			// 		char name[10];
+			// 		sprintf(name, "xapo #%d", i);
+			// 		memcpy(dbg.key, name, sizeof(dbg.key));
+			// 		dbg.value = x_aposteriori[i];
+			// 		orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
 
 			printcounter++;
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
index 459dcc9b9..86d872fd2 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:42 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

@@ -15,7 +15,7 @@
 #include "eye.h"

 #include "mrdivide.h"

 #include "diag.h"

-#include "find.h"

+#include "power.h"

 

 /* Type Definitions */

 

@@ -52,662 +52,696 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
 }

 

 /*

- * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,updVect,z_k,u,x_aposteriori_k,P_aposteriori_k,knownConst)

+ * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)

  */

-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])

+void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const

+  real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T

+  P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T

+  eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T

+  P_aposteriori[144])

 {

-  int32_T udpIndVect_sizes;

-  real_T udpIndVect_data[9];

-  real32_T R_temp[9];

+  real32_T a[12];

+  int32_T i;

+  real32_T b_a[12];

+  real32_T Q[144];

+  real32_T O[9];

   real_T dv0[9];

-  int32_T ib;

-  int32_T i0;

-  real32_T fv0[81];

-  real32_T b_knownConst[27];

-  real32_T fv1[9];

-  real32_T c_knownConst[9];

-  real_T dv1[9];

-  real_T dv2[9];

-  real32_T A_lin[81];

+  real32_T c_a[9];

+  real32_T d_a[9];

   real32_T x_n_b[3];

-  real32_T K_k_data[81];

+  real32_T z_n_b[3];

+  real32_T x_apriori[12];

+  real32_T y_n_b[3];

+  int32_T i0;

+  real32_T e_a[3];

+  real_T dv1[144];

+  real32_T A_lin[144];

+  real32_T b_A_lin[144];

   int32_T i1;

-  real32_T b_A_lin[81];

-  static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };

-

-  real32_T P_apriori[81];

-  int32_T ia;

-  static const int8_T H_k_full[81] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,

+  real32_T y;

+  real32_T P_apriori[144];

+  real32_T R[81];

+  real32_T b_P_apriori[108];

+  static const int8_T iv0[108] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,

+    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

+    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

+    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

+    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

+

+  real32_T K_k[108];

+  static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,

+    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,

     0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

     1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,

     0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

 

-  int8_T H_k_data[81];

-  int32_T accUpt;

-  int32_T magUpt;

-  real32_T y;

-  real32_T y_k_data[9];

-  int32_T P_apriori_sizes[2];

-  int32_T H_k_sizes[2];

-  int32_T K_k_sizes[2];

-  real32_T b_y[9];

-  real_T dv3[81];

-  real32_T c_y;

-  real32_T z_n_b[3];

-  real32_T y_n_b[3];

-

-  /* Extended Attitude Kalmanfilter */

+  real32_T fv0[81];

+  real32_T c_P_apriori[36];

+  static const int8_T iv2[36] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

+    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0 };

+

+  real32_T fv1[36];

+  static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0,

+    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

+

+  real32_T S_k[36];

+  real32_T d_P_apriori[72];

+  static const int8_T iv4[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

+    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

+    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

+    1, 0, 0, 0 };

+

+  real32_T b_K_k[72];

+  static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

+    0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0,

+    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

+    0, 0, 0, 0 };

+

+  real32_T b_r[6];

+  static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

+    0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,

+    0, 0, 0, 1 };

+

+  static const int8_T iv7[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

+    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

+    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

+    0, 0, 0, 1 };

+

+  real32_T fv2[6];

+  real32_T b_z[6];

+  real32_T b_y;

+

+  /*  Extended Attitude Kalmanfilter */

   /*  */

-  /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */

-  /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */

-  /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */

+  /*  state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */

+  /*  measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */

+  /*  knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */

   /*  */

-  /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */

+  /*  [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */

   /*  */

-  /* Example.... */

+  /*  Example.... */

   /*  */

   /*  $Author: Tobias Naegeli $    $Date: 2012 $    $Revision: 1 $ */

-  /* %define the matrices */

-  /*  tpred=0.005; */

-  /*   */

-  /*  A=[   0,   0,  0,  0, 0, 0, 0, 0, 0;  */

-  /*        0,   0,  0,  0, 0, 0, 0, 0, 0; */

-  /*        0,   0,  0,  0, 0, 0, 0, 0, 0;   */

-  /*        0,   -1,  1,  0, 0, 0, 0, 0, 0;   */

-  /*        1,   0,  -1,  0, 0, 0, 0, 0, 0;   */

-  /*        -1,   1,  0,  0, 0, 0, 0, 0, 0; */

-  /*        0,   -1,  1,  0, 0, 0, 0, 0, 0;   */

-  /*        1,   0,  -1,  0, 0, 0, 0, 0, 0;   */

-  /*        -1,   1,  0,  0, 0, 0, 0, 0, 0];  */

-  /*   */

-  /*   */

-  /*  b=[70,   0,   0;   */

-  /*    0, 70,   0;  */

-  /*    0,   0,   0;   */

-  /*    0,   0,   0;   */

-  /*    0,   0, 0;   */

-  /*    0,   0,   0;   */

-  /*    0,   0,   0;   */

-  /*    0,   0,   0; */

-  /*    0,   0,   0];   */

-  /*   */

-  /*   */

-  /*  C=[1,   0,  0,  0, 0, 0, 0, 0, 0; */

-  /*      0,   1,  0,  0, 0, 0, 0, 0, 0;   */

-  /*      0,   0,  1,  0, 0, 0, 0, 0, 0; */

-  /*      0,   0,  0,  1, 0, 0, 0, 0, 0; */

-  /*      0,   0,  0,  0, 1, 0, 0, 0, 0; */

-  /*      0,   0,  0,  0, 0, 1, 0, 0, 0; */

-  /*      0,   0,  0,  0, 0, 0, 1, 0, 0; */

-  /*      0,   0,  0,  0, 0, 0, 0, 1, 0; */

-  /*      0,   0,  0,  0, 0, 0, 0, 0, 1]; */

-  /*  D=[]; */

-  /*   */

-  /*   */

-  /*  sys=ss(A,b,C,D); */

-  /*   */

-  /*  sysd=c2d(sys,tpred); */

-  /*   */

-  /*   */

-  /*  F=sysd.a; */

-  /*   */

-  /*  B=sysd.b; */

-  /*   */

-  /*  H=sysd.c; */

-  /* 'attitudeKalmanfilter:68' udpIndVect=find(updVect); */

-  find(updVect, udpIndVect_data, &udpIndVect_sizes);

-

-  /* 'attitudeKalmanfilter:71' rates_ProcessNoiseMatrix=diag([knownConst(1),knownConst(1),knownConst(2)]); */

-  /* 'attitudeKalmanfilter:72' acc_ProcessNoise=knownConst(3); */

-  /* 'attitudeKalmanfilter:73' mag_ProcessNoise=knownConst(4); */

-  /* 'attitudeKalmanfilter:74' rates_MeasurementNoise=knownConst(6); */

-  /* 'attitudeKalmanfilter:75' acc_MeasurementNoise=knownConst(7); */

-  /* 'attitudeKalmanfilter:76' mag_MeasurementNoise=knownConst(8); */

-  /* process noise covariance matrix */

-  /* 'attitudeKalmanfilter:81' Q = [  rates_ProcessNoiseMatrix,     zeros(3),                                              zeros(3); */

-  /* 'attitudeKalmanfilter:82'     zeros(3),                       eye(3)*mag_ProcessNoise,                               zeros(3); */

-  /* 'attitudeKalmanfilter:83'     zeros(3),                       zeros(3),                                              eye(3)*acc_ProcessNoise]; */

-  /* observation matrix */

-  /* 'attitudeKalmanfilter:89' H_k_full=[   eye(3),     zeros(3),      zeros(3); */

-  /* 'attitudeKalmanfilter:90'     zeros(3),   eye(3),        zeros(3); */

-  /* 'attitudeKalmanfilter:91'     zeros(3),   zeros(3),        eye(3)]; */

-  /* compute A(t,w) */

-  /* x_aposteriori_k[10,11,12] should be [p,q,r] */

-  /* R_temp=[1,-r, q */

-  /*         r, 1, -p */

-  /*        -q, p, 1] */

-  /* 'attitudeKalmanfilter:100' R_temp=[1,                      -dt*x_aposteriori_k(3), dt*x_aposteriori_k(2); */

-  /* 'attitudeKalmanfilter:101'         dt*x_aposteriori_k(3),  1,                      -dt*x_aposteriori_k(1); */

-  /* 'attitudeKalmanfilter:102'         -dt*x_aposteriori_k(2), dt*x_aposteriori_k(1),  1]; */

-  R_temp[0] = 1.0F;

-  R_temp[3] = -dt * x_aposteriori_k[2];

-  R_temp[6] = dt * x_aposteriori_k[1];

-  R_temp[1] = dt * x_aposteriori_k[2];

-  R_temp[4] = 1.0F;

-  R_temp[7] = -dt * x_aposteriori_k[0];

-  R_temp[2] = -dt * x_aposteriori_k[1];

-  R_temp[5] = dt * x_aposteriori_k[0];

-  R_temp[8] = 1.0F;

-

-  /* 'attitudeKalmanfilter:106' A_pred=[eye(3),     zeros(3),       zeros(3); */

-  /* 'attitudeKalmanfilter:107'         zeros(3),   R_temp',        zeros(3); */

-  /* 'attitudeKalmanfilter:108'         zeros(3),   zeros(3),       R_temp']; */

-  /* 'attitudeKalmanfilter:110' B_pred=[knownConst(9)*dt,   0,                  0; */

-  /* 'attitudeKalmanfilter:111'         0,                  knownConst(10)*dt,  0; */

-  /* 'attitudeKalmanfilter:112'         0,                  0,                  0; */

-  /* 'attitudeKalmanfilter:113'         0,                  0,                  0; */

-  /* 'attitudeKalmanfilter:114'         0,                  0,                  0; */

-  /* 'attitudeKalmanfilter:115'         0,                  0,                  0; */

-  /* 'attitudeKalmanfilter:116'         0,                  0,                  0; */

-  /* 'attitudeKalmanfilter:117'         0,                  0,                  0; */

-  /* 'attitudeKalmanfilter:118'         0,                  0,                  0]; */

-  /* %prediction step */

-  /* 'attitudeKalmanfilter:121' x_apriori=A_pred*x_aposteriori_k+B_pred*u(1:3); */

-  eye(dv0);

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      fv0[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib];

-    }

-  }

-

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      fv0[i0 + 9 * (ib + 3)] = 0.0F;

-    }

+  /* coder.varsize('udpIndVect', [9,1], [1,0]) */

+  /* udpIndVect=find(updVect); */

+  /* process and measurement noise covariance matrix */

+  /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */

+  power(q, 2.0, a);

+  for (i = 0; i < 12; i++) {

+    b_a[i] = a[i] * dt;

   }

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      fv0[i0 + 9 * (ib + 6)] = 0.0F;

-    }

-  }

+  diag(b_a, Q);

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      fv0[(i0 + 9 * ib) + 3] = 0.0F;

-    }

+  /* observation matrix */

+  /* 'attitudeKalmanfilter:37' wx=  x_aposteriori_k(1); */

+  /* 'attitudeKalmanfilter:38' wy=  x_aposteriori_k(2); */

+  /* 'attitudeKalmanfilter:39' wz=  x_aposteriori_k(3); */

+  /* 'attitudeKalmanfilter:41' wox=  x_aposteriori_k(4); */

+  /* 'attitudeKalmanfilter:42' woy=  x_aposteriori_k(5); */

+  /* 'attitudeKalmanfilter:43' woz=  x_aposteriori_k(6); */

+  /* 'attitudeKalmanfilter:45' zex=  x_aposteriori_k(7); */

+  /* 'attitudeKalmanfilter:46' zey=  x_aposteriori_k(8); */

+  /* 'attitudeKalmanfilter:47' zez=  x_aposteriori_k(9); */

+  /* 'attitudeKalmanfilter:49' mux=  x_aposteriori_k(10); */

+  /* 'attitudeKalmanfilter:50' muy=  x_aposteriori_k(11); */

+  /* 'attitudeKalmanfilter:51' muz=  x_aposteriori_k(12); */

+  /* 'attitudeKalmanfilter:54' wk =[wx; */

+  /* 'attitudeKalmanfilter:55'      wy; */

+  /* 'attitudeKalmanfilter:56'      wz]; */

+  /* 'attitudeKalmanfilter:58' wok =[wox;woy;woz]; */

+  /* 'attitudeKalmanfilter:59' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */

+  O[0] = 0.0F;

+  O[1] = -x_aposteriori_k[2];

+  O[2] = x_aposteriori_k[1];

+  O[3] = x_aposteriori_k[2];

+  O[4] = 0.0F;

+  O[5] = -x_aposteriori_k[0];

+  O[6] = -x_aposteriori_k[1];

+  O[7] = x_aposteriori_k[0];

+  O[8] = 0.0F;

+

+  /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */

+  eye(dv0);

+  for (i = 0; i < 9; i++) {

+    c_a[i] = (real32_T)dv0[i] + O[i] * dt;

   }

 

-  for (ib = 0; ib < 3; ib++) {

+  /* 'attitudeKalmanfilter:61' muk =(eye(3)+O*dt)*[mux;muy;muz]; */

+  eye(dv0);

+  for (i = 0; i < 9; i++) {

+    d_a[i] = (real32_T)dv0[i] + O[i] * dt;

+  }

+

+  /* 'attitudeKalmanfilter:63' EZ=[0,zez,-zey; */

+  /* 'attitudeKalmanfilter:64'     -zez,0,zex; */

+  /* 'attitudeKalmanfilter:65'     zey,-zex,0]'; */

+  /* 'attitudeKalmanfilter:66' MA=[0,muz,-muy; */

+  /* 'attitudeKalmanfilter:67'     -muz,0,mux; */

+  /* 'attitudeKalmanfilter:68'     zey,-mux,0]'; */

+  /* 'attitudeKalmanfilter:72' E=eye(3); */

+  /* 'attitudeKalmanfilter:73' Z=zeros(3); */

+  /* 'attitudeKalmanfilter:74' x_apriori=[wk;wok;zek;muk]; */

+  x_n_b[0] = x_aposteriori_k[6];

+  x_n_b[1] = x_aposteriori_k[7];

+  x_n_b[2] = x_aposteriori_k[8];

+  z_n_b[0] = x_aposteriori_k[9];

+  z_n_b[1] = x_aposteriori_k[10];

+  z_n_b[2] = x_aposteriori_k[11];

+  x_apriori[0] = x_aposteriori_k[0];

+  x_apriori[1] = x_aposteriori_k[1];

+  x_apriori[2] = x_aposteriori_k[2];

+  x_apriori[3] = x_aposteriori_k[3];

+  x_apriori[4] = x_aposteriori_k[4];

+  x_apriori[5] = x_aposteriori_k[5];

+  for (i = 0; i < 3; i++) {

+    y_n_b[i] = 0.0F;

     for (i0 = 0; i0 < 3; i0++) {

-      fv0[(i0 + 9 * (ib + 3)) + 3] = R_temp[ib + 3 * i0];

+      y_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0];

     }

-  }

 

-  for (ib = 0; ib < 3; ib++) {

+    e_a[i] = 0.0F;

     for (i0 = 0; i0 < 3; i0++) {

-      fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F;

+      e_a[i] += d_a[i + 3 * i0] * z_n_b[i0];

     }

-  }

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      fv0[(i0 + 9 * ib) + 6] = 0.0F;

-    }

+    x_apriori[i + 6] = y_n_b[i];

   }

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F;

-    }

+  for (i = 0; i < 3; i++) {

+    x_apriori[i + 9] = e_a[i];

   }

 

-  b_knownConst[0] = knownConst[8] * dt;

-  b_knownConst[9] = 0.0F;

-  b_knownConst[18] = 0.0F;

-  b_knownConst[1] = 0.0F;

-  b_knownConst[10] = knownConst[9] * dt;

-  b_knownConst[19] = 0.0F;

-  for (ib = 0; ib < 3; ib++) {

+  /* 'attitudeKalmanfilter:76' A_lin=[ Z,  Z,  Z,  Z */

+  /* 'attitudeKalmanfilter:77'         Z,  Z,  Z,  Z */

+  /* 'attitudeKalmanfilter:78'         EZ, Z,  O,  Z */

+  /* 'attitudeKalmanfilter:79'         MA, Z,  Z,  O]; */

+  /* 'attitudeKalmanfilter:82' A_lin=eye(12)+A_lin*dt; */

+  b_eye(dv1);

+  for (i = 0; i < 12; i++) {

     for (i0 = 0; i0 < 3; i0++) {

-      fv0[(i0 + 9 * (ib + 6)) + 6] = R_temp[ib + 3 * i0];

+      A_lin[i0 + 12 * i] = 0.0F;

     }

 

-    b_knownConst[2 + 9 * ib] = 0.0F;

-    b_knownConst[3 + 9 * ib] = 0.0F;

-    b_knownConst[4 + 9 * ib] = 0.0F;

-    b_knownConst[5 + 9 * ib] = 0.0F;

-    b_knownConst[6 + 9 * ib] = 0.0F;

-    b_knownConst[7 + 9 * ib] = 0.0F;

-    b_knownConst[8 + 9 * ib] = 0.0F;

-  }

-

-  for (ib = 0; ib < 9; ib++) {

-    fv1[ib] = 0.0F;

-    for (i0 = 0; i0 < 9; i0++) {

-      fv1[ib] += fv0[ib + 9 * i0] * x_aposteriori_k[i0];

-    }

-

-    c_knownConst[ib] = 0.0F;

     for (i0 = 0; i0 < 3; i0++) {

-      c_knownConst[ib] += b_knownConst[ib + 9 * i0] * u[i0];

+      A_lin[(i0 + 12 * i) + 3] = 0.0F;

     }

-

-    x_aposteriori[ib] = fv1[ib] + c_knownConst[ib];

   }

 

-  /* linearization */

-  /* 'attitudeKalmanfilter:125' temp_mat=[  0,      -dt,    dt; */

-  /* 'attitudeKalmanfilter:126'             dt,    0,     -dt; */

-  /* 'attitudeKalmanfilter:127'             -dt,     dt,   0]; */

-  R_temp[0] = 0.0F;

-  R_temp[3] = -dt;

-  R_temp[6] = dt;

-  R_temp[1] = dt;

-  R_temp[4] = 0.0F;

-  R_temp[7] = -dt;

-  R_temp[2] = -dt;

-  R_temp[5] = dt;

-  R_temp[8] = 0.0F;

-

-  /* 'attitudeKalmanfilter:129' A_lin=[ eye(3),         zeros(3),     zeros(3); */

-  /* 'attitudeKalmanfilter:130'         temp_mat,  eye(3) ,     zeros(3); */

-  /* 'attitudeKalmanfilter:131'         temp_mat,  zeros(3),     eye(3)]; */

-  eye(dv0);

-  eye(dv1);

-  eye(dv2);

-  for (ib = 0; ib < 3; ib++) {

+  A_lin[6] = 0.0F;

+  A_lin[7] = x_aposteriori_k[8];

+  A_lin[8] = -x_aposteriori_k[7];

+  A_lin[18] = -x_aposteriori_k[8];

+  A_lin[19] = 0.0F;

+  A_lin[20] = x_aposteriori_k[6];

+  A_lin[30] = x_aposteriori_k[7];

+  A_lin[31] = -x_aposteriori_k[6];

+  A_lin[32] = 0.0F;

+  for (i = 0; i < 3; i++) {

     for (i0 = 0; i0 < 3; i0++) {

-      A_lin[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib];

+      A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;

     }

   }

 

-  for (ib = 0; ib < 3; ib++) {

+  for (i = 0; i < 3; i++) {

     for (i0 = 0; i0 < 3; i0++) {

-      A_lin[i0 + 9 * (ib + 3)] = 0.0F;

+      A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];

     }

   }

 

-  for (ib = 0; ib < 3; ib++) {

+  for (i = 0; i < 3; i++) {

     for (i0 = 0; i0 < 3; i0++) {

-      A_lin[i0 + 9 * (ib + 6)] = 0.0F;

+      A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;

     }

   }

 

-  for (ib = 0; ib < 3; ib++) {

+  A_lin[9] = 0.0F;

+  A_lin[10] = x_aposteriori_k[11];

+  A_lin[11] = -x_aposteriori_k[10];

+  A_lin[21] = -x_aposteriori_k[11];

+  A_lin[22] = 0.0F;

+  A_lin[23] = x_aposteriori_k[9];

+  A_lin[33] = x_aposteriori_k[7];

+  A_lin[34] = -x_aposteriori_k[9];

+  A_lin[35] = 0.0F;

+  for (i = 0; i < 3; i++) {

     for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 9 * ib) + 3] = R_temp[i0 + 3 * ib];

+      A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;

     }

   }

 

-  for (ib = 0; ib < 3; ib++) {

+  for (i = 0; i < 3; i++) {

     for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 9 * (ib + 3)) + 3] = (real32_T)dv1[i0 + 3 * ib];

+      A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;

     }

   }

 

-  for (ib = 0; ib < 3; ib++) {

+  for (i = 0; i < 3; i++) {

     for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 9 * (ib + 6)) + 3] = 0.0F;

+      A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];

     }

   }

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 9 * ib) + 6] = R_temp[i0 + 3 * ib];

+  for (i = 0; i < 12; i++) {

+    for (i0 = 0; i0 < 12; i0++) {

+      b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *

+        dt;

     }

   }

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 9 * (ib + 3)) + 6] = 0.0F;

-    }

-  }

-

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 9 * (ib + 6)) + 6] = (real32_T)dv2[i0 + 3 * ib];

-    }

-  }

-

-  /* 'attitudeKalmanfilter:134' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */

-  x_n_b[0] = knownConst[0];

-  x_n_b[1] = knownConst[0];

-  x_n_b[2] = knownConst[1];

-  diag(x_n_b, R_temp);

-  for (ib = 0; ib < 9; ib++) {

-    for (i0 = 0; i0 < 9; i0++) {

-      K_k_data[ib + 9 * i0] = 0.0F;

-      for (i1 = 0; i1 < 9; i1++) {

-        K_k_data[ib + 9 * i0] += A_lin[ib + 9 * i1] * P_aposteriori_k[i1 + 9 *

+  /* 'attitudeKalmanfilter:88' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */

+  for (i = 0; i < 12; i++) {

+    for (i0 = 0; i0 < 12; i0++) {

+      A_lin[i + 12 * i0] = 0.0F;

+      for (i1 = 0; i1 < 12; i1++) {

+        A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *

           i0];

       }

     }

-

-    for (i0 = 0; i0 < 9; i0++) {

-      b_A_lin[ib + 9 * i0] = 0.0F;

-      for (i1 = 0; i1 < 9; i1++) {

-        b_A_lin[ib + 9 * i0] += K_k_data[ib + 9 * i1] * A_lin[i0 + 9 * i1];

-      }

-    }

-  }

-

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      K_k_data[i0 + 9 * ib] = R_temp[i0 + 3 * ib];

-    }

   }

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      K_k_data[i0 + 9 * (ib + 3)] = 0.0F;

-    }

-  }

-

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      K_k_data[i0 + 9 * (ib + 6)] = 0.0F;

-    }

-  }

-

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      K_k_data[(i0 + 9 * ib) + 3] = 0.0F;

-    }

-  }

-

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      K_k_data[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] *

-        knownConst[3];

-    }

-  }

+  for (i = 0; i < 12; i++) {

+    for (i0 = 0; i0 < 12; i0++) {

+      y = 0.0F;

+      for (i1 = 0; i1 < 12; i1++) {

+        y += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

+      }

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      K_k_data[(i0 + 9 * (ib + 6)) + 3] = 0.0F;

+      P_apriori[i + 12 * i0] = y + Q[i + 12 * i0];

     }

   }

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      K_k_data[(i0 + 9 * ib) + 6] = 0.0F;

-    }

-  }

+  /* %update */

+  /* 'attitudeKalmanfilter:92' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */

+  if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {

+    /* 'attitudeKalmanfilter:93' R=diag(r); */

+    b_diag(r, R);

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      K_k_data[(i0 + 9 * (ib + 3)) + 6] = 0.0F;

+    /* observation matrix */

+    /* 'attitudeKalmanfilter:96' H_k=[  E,     E,      Z,    Z; */

+    /* 'attitudeKalmanfilter:97'         Z,     Z,      E,    Z; */

+    /* 'attitudeKalmanfilter:98'         Z,     Z,      Z,    E]; */

+    /* 'attitudeKalmanfilter:100' y_k=z(1:9)-H_k*x_apriori; */

+    /* 'attitudeKalmanfilter:102' S_k=H_k*P_apriori*H_k'+R; */

+    /* 'attitudeKalmanfilter:103' K_k=(P_apriori*H_k'/(S_k)); */

+    for (i = 0; i < 12; i++) {

+      for (i0 = 0; i0 < 9; i0++) {

+        b_P_apriori[i + 12 * i0] = 0.0F;

+        for (i1 = 0; i1 < 12; i1++) {

+          b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv0[i1

+            + 12 * i0];

+        }

+      }

     }

-  }

 

-  for (ib = 0; ib < 3; ib++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      K_k_data[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] *

-        knownConst[2];

+    for (i = 0; i < 9; i++) {

+      for (i0 = 0; i0 < 12; i0++) {

+        K_k[i + 9 * i0] = 0.0F;

+        for (i1 = 0; i1 < 12; i1++) {

+          K_k[i + 9 * i0] += (real32_T)iv1[i + 9 * i1] * P_apriori[i1 + 12 * i0];

+        }

+      }

     }

-  }

 

-  for (ib = 0; ib < 9; ib++) {

-    for (i0 = 0; i0 < 9; i0++) {

-      P_apriori[i0 + 9 * ib] = b_A_lin[i0 + 9 * ib] + K_k_data[i0 + 9 * ib];

-    }

-  }

+    for (i = 0; i < 9; i++) {

+      for (i0 = 0; i0 < 9; i0++) {

+        y = 0.0F;

+        for (i1 = 0; i1 < 12; i1++) {

+          y += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0];

+        }

 

-  /* 'attitudeKalmanfilter:137' if ~isempty(udpIndVect)==1 */

-  if (!(udpIndVect_sizes == 0) == 1) {

-    /* 'attitudeKalmanfilter:138' H_k= H_k_full(udpIndVect,:); */

-    for (ib = 0; ib < 9; ib++) {

-      ia = udpIndVect_sizes - 1;

-      for (i0 = 0; i0 <= ia; i0++) {

-        H_k_data[i0 + udpIndVect_sizes * ib] = H_k_full[((int32_T)

-          udpIndVect_data[i0] + 9 * ib) - 1];

+        fv0[i + 9 * i0] = y + R[i + 9 * i0];

       }

     }

 

-    /* %update step */

-    /* 'attitudeKalmanfilter:140' accUpt=1; */

-    accUpt = 1;

+    mrdivide(b_P_apriori, fv0, K_k);

 

-    /* 'attitudeKalmanfilter:141' magUpt=1; */

-    magUpt = 1;

-

-    /* 'attitudeKalmanfilter:142' y_k=z_k-H_k*x_apriori; */

-    ia = udpIndVect_sizes - 1;

-    for (ib = 0; ib <= ia; ib++) {

+    /* 'attitudeKalmanfilter:106' x_aposteriori=x_apriori+K_k*y_k; */

+    for (i = 0; i < 9; i++) {

       y = 0.0F;

-      for (i0 = 0; i0 < 9; i0++) {

-        y += (real32_T)H_k_data[ib + udpIndVect_sizes * i0] * x_aposteriori[i0];

+      for (i0 = 0; i0 < 12; i0++) {

+        y += (real32_T)iv1[i + 9 * i0] * x_apriori[i0];

       }

 

-      y_k_data[ib] = z_k_data[ib] - y;

+      c_a[i] = z[i] - y;

     }

 

-    /* 'attitudeKalmanfilter:143' if updVect(4)==1 */

-    if (updVect[3] == 1) {

-      /* 'attitudeKalmanfilter:144' if (abs(norm(z_k(4:6))-knownConst(12))>knownConst(14)) */

-      for (ib = 0; ib < 3; ib++) {

-        x_n_b[ib] = z_k_data[ib + 3];

+    for (i = 0; i < 12; i++) {

+      y = 0.0F;

+      for (i0 = 0; i0 < 9; i0++) {

+        y += K_k[i + 12 * i0] * c_a[i0];

       }

 

-      if ((real32_T)fabsf(norm(x_n_b) - knownConst[11]) > knownConst[13]) {

-        /* 'attitudeKalmanfilter:145' accUpt=10000; */

-        accUpt = 10000;

-      }

+      x_aposteriori[i] = x_apriori[i] + y;

     }

 

-    /* 'attitudeKalmanfilter:149' if updVect(7)==1 */

-    if (updVect[6] == 1) {

-      /* 'attitudeKalmanfilter:150' if (abs(norm(z_k(7:9))-knownConst(13))>knownConst(15)) */

-      for (ib = 0; ib < 3; ib++) {

-        x_n_b[ib] = z_k_data[ib + 6];

-      }

+    /* 'attitudeKalmanfilter:107' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */

+    b_eye(dv1);

+    for (i = 0; i < 12; i++) {

+      for (i0 = 0; i0 < 12; i0++) {

+        y = 0.0F;

+        for (i1 = 0; i1 < 9; i1++) {

+          y += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0];

+        }

 

-      if ((real32_T)fabs(norm(x_n_b) - knownConst[12]) > knownConst[14]) {

-        /* 'attitudeKalmanfilter:152' magUpt=10000; */

-        magUpt = 10000;

+        Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;

       }

     }

 

-    /* measurement noise covariance matrix */

-    /* 'attitudeKalmanfilter:157' R = [   eye(3)*rates_MeasurementNoise,       zeros(3),                       zeros(3); */

-    /* 'attitudeKalmanfilter:158'             zeros(3),                            eye(3)*acc_MeasurementNoise*accUpt,    zeros(3); */

-    /* 'attitudeKalmanfilter:159'             zeros(3),                            zeros(3),                       eye(3)*mag_MeasurementNoise*magUpt]; */

-    /* 'attitudeKalmanfilter:161' S_k=H_k*P_apriori*H_k'+R(udpIndVect,udpIndVect); */

-    /* 'attitudeKalmanfilter:162' K_k=(P_apriori*H_k'/(S_k)); */

-    P_apriori_sizes[0] = 9;

-    P_apriori_sizes[1] = udpIndVect_sizes;

-    for (ib = 0; ib < 9; ib++) {

-      ia = udpIndVect_sizes - 1;

-      for (i0 = 0; i0 <= ia; i0++) {

-        b_A_lin[ib + 9 * i0] = 0.0F;

-        for (i1 = 0; i1 < 9; i1++) {

-          b_A_lin[ib + 9 * i0] += P_apriori[ib + 9 * i1] * (real32_T)H_k_data[i0

-            + udpIndVect_sizes * i1];

+    for (i = 0; i < 12; i++) {

+      for (i0 = 0; i0 < 12; i0++) {

+        P_aposteriori[i + 12 * i0] = 0.0F;

+        for (i1 = 0; i1 < 12; i1++) {

+          P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0];

         }

       }

     }

-

-    ia = udpIndVect_sizes - 1;

-    for (ib = 0; ib <= ia; ib++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        K_k_data[ib + udpIndVect_sizes * i0] = 0.0F;

-        for (i1 = 0; i1 < 9; i1++) {

-          K_k_data[ib + udpIndVect_sizes * i0] += (real32_T)H_k_data[ib +

-            udpIndVect_sizes * i1] * P_apriori[i1 + 9 * i0];

+  } else {

+    /* 'attitudeKalmanfilter:108' else */

+    /* 'attitudeKalmanfilter:109' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */

+    if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {

+      /* 'attitudeKalmanfilter:110' R=diag(r(1:3)); */

+      c_diag(*(real32_T (*)[3])&r[0], O);

+

+      /* observation matrix */

+      /* 'attitudeKalmanfilter:113' H_k=[  E,     E,      Z,    Z]; */

+      /* 'attitudeKalmanfilter:115' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */

+      /* 'attitudeKalmanfilter:117' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */

+      /* 'attitudeKalmanfilter:118' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */

+      for (i = 0; i < 12; i++) {

+        for (i0 = 0; i0 < 3; i0++) {

+          c_P_apriori[i + 12 * i0] = 0.0F;

+          for (i1 = 0; i1 < 12; i1++) {

+            c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

+              iv2[i1 + 12 * i0];

+          }

         }

       }

-    }

 

-    for (ib = 0; ib < 3; ib++) {

-      for (i0 = 0; i0 < 3; i0++) {

-        fv0[i0 + 9 * ib] = (real32_T)iv0[i0 + 3 * ib] * knownConst[5];

+      for (i = 0; i < 3; i++) {

+        for (i0 = 0; i0 < 12; i0++) {

+          fv1[i + 3 * i0] = 0.0F;

+          for (i1 = 0; i1 < 12; i1++) {

+            fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 *

+              i0];

+          }

+        }

       }

-    }

 

-    for (ib = 0; ib < 3; ib++) {

-      for (i0 = 0; i0 < 3; i0++) {

-        fv0[i0 + 9 * (ib + 3)] = 0.0F;

-      }

-    }

+      for (i = 0; i < 3; i++) {

+        for (i0 = 0; i0 < 3; i0++) {

+          y = 0.0F;

+          for (i1 = 0; i1 < 12; i1++) {

+            y += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];

+          }

 

-    for (ib = 0; ib < 3; ib++) {

-      for (i0 = 0; i0 < 3; i0++) {

-        fv0[i0 + 9 * (ib + 6)] = 0.0F;

+          c_a[i + 3 * i0] = y + O[i + 3 * i0];

+        }

       }

-    }

 

-    for (ib = 0; ib < 3; ib++) {

-      for (i0 = 0; i0 < 3; i0++) {

-        fv0[(i0 + 9 * ib) + 3] = 0.0F;

-      }

-    }

+      b_mrdivide(c_P_apriori, c_a, S_k);

 

-    for (ib = 0; ib < 3; ib++) {

-      for (i0 = 0; i0 < 3; i0++) {

-        fv0[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[6]

-          * (real32_T)accUpt;

-      }

-    }

+      /* 'attitudeKalmanfilter:121' x_aposteriori=x_apriori+K_k*y_k; */

+      for (i = 0; i < 3; i++) {

+        y = 0.0F;

+        for (i0 = 0; i0 < 12; i0++) {

+          y += (real32_T)iv3[i + 3 * i0] * x_apriori[i0];

+        }

 

-    for (ib = 0; ib < 3; ib++) {

-      for (i0 = 0; i0 < 3; i0++) {

-        fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F;

+        x_n_b[i] = z[i] - y;

       }

-    }

 

-    for (ib = 0; ib < 3; ib++) {

-      for (i0 = 0; i0 < 3; i0++) {

-        fv0[(i0 + 9 * ib) + 6] = 0.0F;

+      for (i = 0; i < 12; i++) {

+        y = 0.0F;

+        for (i0 = 0; i0 < 3; i0++) {

+          y += S_k[i + 12 * i0] * x_n_b[i0];

+        }

+

+        x_aposteriori[i] = x_apriori[i] + y;

       }

-    }

 

-    for (ib = 0; ib < 3; ib++) {

-      for (i0 = 0; i0 < 3; i0++) {

-        fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F;

+      /* 'attitudeKalmanfilter:122' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */

+      b_eye(dv1);

+      for (i = 0; i < 12; i++) {

+        for (i0 = 0; i0 < 12; i0++) {

+          y = 0.0F;

+          for (i1 = 0; i1 < 3; i1++) {

+            y += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0];

+          }

+

+          Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;

+        }

       }

-    }

 

-    for (ib = 0; ib < 3; ib++) {

-      for (i0 = 0; i0 < 3; i0++) {

-        fv0[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * knownConst[7]

-          * (real32_T)magUpt;

+      for (i = 0; i < 12; i++) {

+        for (i0 = 0; i0 < 12; i0++) {

+          P_aposteriori[i + 12 * i0] = 0.0F;

+          for (i1 = 0; i1 < 12; i1++) {

+            P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *

+              i0];

+          }

+        }

       }

-    }

+    } else {

+      /* 'attitudeKalmanfilter:123' else */

+      /* 'attitudeKalmanfilter:124' if  updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */

+      if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))

+      {

+        /* 'attitudeKalmanfilter:125' R=diag(r(1:6)); */

+        d_diag(*(real32_T (*)[6])&r[0], S_k);

+

+        /* observation matrix */

+        /* 'attitudeKalmanfilter:128' H_k=[  E,     E,      Z,    Z; */

+        /* 'attitudeKalmanfilter:129'                 Z,     Z,      E,    Z]; */

+        /* 'attitudeKalmanfilter:131' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */

+        /* 'attitudeKalmanfilter:133' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

+        /* 'attitudeKalmanfilter:134' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

+        for (i = 0; i < 12; i++) {

+          for (i0 = 0; i0 < 6; i0++) {

+            d_P_apriori[i + 12 * i0] = 0.0F;

+            for (i1 = 0; i1 < 12; i1++) {

+              d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

+                iv4[i1 + 12 * i0];

+            }

+          }

+        }

 

-    H_k_sizes[0] = udpIndVect_sizes;

-    H_k_sizes[1] = udpIndVect_sizes;

-    ia = udpIndVect_sizes - 1;

-    for (ib = 0; ib <= ia; ib++) {

-      accUpt = udpIndVect_sizes - 1;

-      for (i0 = 0; i0 <= accUpt; i0++) {

-        y = 0.0F;

-        for (i1 = 0; i1 < 9; i1++) {

-          y += K_k_data[ib + udpIndVect_sizes * i1] * (real32_T)H_k_data[i0 +

-            udpIndVect_sizes * i1];

+        for (i = 0; i < 6; i++) {

+          for (i0 = 0; i0 < 12; i0++) {

+            b_K_k[i + 6 * i0] = 0.0F;

+            for (i1 = 0; i1 < 12; i1++) {

+              b_K_k[i + 6 * i0] += (real32_T)iv5[i + 6 * i1] * P_apriori[i1 + 12

+                * i0];

+            }

+          }

         }

 

-        A_lin[ib + H_k_sizes[0] * i0] = y + fv0[((int32_T)udpIndVect_data[ib] +

-          9 * ((int32_T)udpIndVect_data[i0] - 1)) - 1];

-      }

-    }

+        for (i = 0; i < 6; i++) {

+          for (i0 = 0; i0 < 6; i0++) {

+            y = 0.0F;

+            for (i1 = 0; i1 < 12; i1++) {

+              y += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0];

+            }

+

+            fv1[i + 6 * i0] = y + S_k[i + 6 * i0];

+          }

+        }

+

+        c_mrdivide(d_P_apriori, fv1, b_K_k);

 

-    mrdivide(b_A_lin, P_apriori_sizes, A_lin, H_k_sizes, K_k_data, K_k_sizes);

+        /* 'attitudeKalmanfilter:137' x_aposteriori=x_apriori+K_k*y_k; */

+        for (i = 0; i < 6; i++) {

+          y = 0.0F;

+          for (i0 = 0; i0 < 12; i0++) {

+            y += (real32_T)iv5[i + 6 * i0] * x_apriori[i0];

+          }

 

-    /* 'attitudeKalmanfilter:165' x_aposteriori=x_apriori+K_k*y_k; */

-    if ((K_k_sizes[1] == 1) || (udpIndVect_sizes == 1)) {

-      for (ib = 0; ib < 9; ib++) {

-        b_y[ib] = 0.0F;

-        ia = udpIndVect_sizes - 1;

-        for (i0 = 0; i0 <= ia; i0++) {

-          b_y[ib] += K_k_data[ib + K_k_sizes[0] * i0] * y_k_data[i0];

+          b_r[i] = z[i] - y;

         }

-      }

-    } else {

-      for (accUpt = 0; accUpt < 9; accUpt++) {

-        b_y[accUpt] = 0.0F;

-      }

 

-      magUpt = -1;

-      for (ib = 0; ib + 1 <= K_k_sizes[1]; ib++) {

-        if ((real_T)y_k_data[ib] != 0.0) {

-          ia = magUpt;

-          for (accUpt = 0; accUpt < 9; accUpt++) {

-            ia++;

-            b_y[accUpt] += y_k_data[ib] * K_k_data[ia];

+        for (i = 0; i < 12; i++) {

+          y = 0.0F;

+          for (i0 = 0; i0 < 6; i0++) {

+            y += b_K_k[i + 12 * i0] * b_r[i0];

           }

+

+          x_aposteriori[i] = x_apriori[i] + y;

         }

 

-        magUpt += 9;

-      }

-    }

+        /* 'attitudeKalmanfilter:138' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

+        b_eye(dv1);

+        for (i = 0; i < 12; i++) {

+          for (i0 = 0; i0 < 12; i0++) {

+            y = 0.0F;

+            for (i1 = 0; i1 < 6; i1++) {

+              y += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0];

+            }

 

-    for (ib = 0; ib < 9; ib++) {

-      x_aposteriori[ib] += b_y[ib];

-    }

+            Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;

+          }

+        }

 

-    /* 'attitudeKalmanfilter:166' P_aposteriori=(eye(9)-K_k*H_k)*P_apriori; */

-    b_eye(dv3);

-    for (ib = 0; ib < 9; ib++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        y = 0.0F;

-        ia = K_k_sizes[1] - 1;

-        for (i1 = 0; i1 <= ia; i1++) {

-          y += K_k_data[ib + K_k_sizes[0] * i1] * (real32_T)H_k_data[i1 +

-            udpIndVect_sizes * i0];

+        for (i = 0; i < 12; i++) {

+          for (i0 = 0; i0 < 12; i0++) {

+            P_aposteriori[i + 12 * i0] = 0.0F;

+            for (i1 = 0; i1 < 12; i1++) {

+              P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *

+                i0];

+            }

+          }

         }

+      } else {

+        /* 'attitudeKalmanfilter:139' else */

+        /* 'attitudeKalmanfilter:140' if  updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */

+        if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))

+        {

+          /* 'attitudeKalmanfilter:141' R=diag([r(1:3);r(7:9)]); */

+          /* observation matrix */

+          /* 'attitudeKalmanfilter:144' H_k=[  E,     E,      Z,    Z; */

+          /* 'attitudeKalmanfilter:145'                     Z,     Z,      Z,    E]; */

+          /* 'attitudeKalmanfilter:147' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */

+          /* 'attitudeKalmanfilter:149' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

+          for (i = 0; i < 6; i++) {

+            for (i0 = 0; i0 < 12; i0++) {

+              b_K_k[i + 6 * i0] = 0.0F;

+              for (i1 = 0; i1 < 12; i1++) {

+                b_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 +

+                  12 * i0];

+              }

+            }

+          }

 

-        fv0[ib + 9 * i0] = (real32_T)dv3[ib + 9 * i0] - y;

-      }

-    }

+          for (i = 0; i < 3; i++) {

+            b_r[i << 1] = r[i];

+            b_r[1 + (i << 1)] = r[6 + i];

+          }

 

-    for (ib = 0; ib < 9; ib++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        P_aposteriori[ib + 9 * i0] = 0.0F;

-        for (i1 = 0; i1 < 9; i1++) {

-          P_aposteriori[ib + 9 * i0] += fv0[ib + 9 * i1] * P_apriori[i1 + 9 * i0];

+          for (i = 0; i < 6; i++) {

+            for (i0 = 0; i0 < 6; i0++) {

+              y = 0.0F;

+              for (i1 = 0; i1 < 12; i1++) {

+                y += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0];

+              }

+

+              S_k[i + 6 * i0] = y + b_r[3 * (i + i0)];

+            }

+          }

+

+          /* 'attitudeKalmanfilter:150' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

+          for (i = 0; i < 12; i++) {

+            for (i0 = 0; i0 < 6; i0++) {

+              d_P_apriori[i + 12 * i0] = 0.0F;

+              for (i1 = 0; i1 < 12; i1++) {

+                d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

+                  iv7[i1 + 12 * i0];

+              }

+            }

+          }

+

+          c_mrdivide(d_P_apriori, S_k, b_K_k);

+

+          /* 'attitudeKalmanfilter:153' x_aposteriori=x_apriori+K_k*y_k; */

+          for (i = 0; i < 3; i++) {

+            b_r[i] = z[i];

+          }

+

+          for (i = 0; i < 3; i++) {

+            b_r[i + 3] = z[i + 6];

+          }

+

+          for (i = 0; i < 6; i++) {

+            fv2[i] = 0.0F;

+            for (i0 = 0; i0 < 12; i0++) {

+              fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];

+            }

+

+            b_z[i] = b_r[i] - fv2[i];

+          }

+

+          for (i = 0; i < 12; i++) {

+            y = 0.0F;

+            for (i0 = 0; i0 < 6; i0++) {

+              y += b_K_k[i + 12 * i0] * b_z[i0];

+            }

+

+            x_aposteriori[i] = x_apriori[i] + y;

+          }

+

+          /* 'attitudeKalmanfilter:154' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

+          b_eye(dv1);

+          for (i = 0; i < 12; i++) {

+            for (i0 = 0; i0 < 12; i0++) {

+              y = 0.0F;

+              for (i1 = 0; i1 < 6; i1++) {

+                y += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];

+              }

+

+              Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;

+            }

+          }

+

+          for (i = 0; i < 12; i++) {

+            for (i0 = 0; i0 < 12; i0++) {

+              P_aposteriori[i + 12 * i0] = 0.0F;

+              for (i1 = 0; i1 < 12; i1++) {

+                P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12

+                  * i0];

+              }

+            }

+          }

+        } else {

+          /* 'attitudeKalmanfilter:155' else */

+          /* 'attitudeKalmanfilter:156' x_aposteriori=x_apriori; */

+          for (i = 0; i < 12; i++) {

+            x_aposteriori[i] = x_apriori[i];

+          }

+

+          /* 'attitudeKalmanfilter:157' P_aposteriori=P_apriori; */

+          memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 144U * sizeof

+                 (real32_T));

         }

       }

     }

-  } else {

-    /* 'attitudeKalmanfilter:167' else */

-    /* 'attitudeKalmanfilter:168' x_aposteriori=x_apriori; */

-    /* 'attitudeKalmanfilter:169' P_aposteriori=P_apriori; */

-    memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 81U * sizeof

-           (real32_T));

   }

 

-  /*         %% euler anglels extraction */

-  /* 'attitudeKalmanfilter:175' z_n_b = -x_aposteriori(4:6)./norm(x_aposteriori(4:6)); */

-  y = norm(*(real32_T (*)[3])&x_aposteriori[3]);

+  /* % euler anglels extraction */

+  /* 'attitudeKalmanfilter:166' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */

+  y = norm(*(real32_T (*)[3])&x_aposteriori[6]);

 

-  /* 'attitudeKalmanfilter:176' m_n_b = x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */

-  c_y = norm(*(real32_T (*)[3])&x_aposteriori[6]);

+  /* 'attitudeKalmanfilter:167' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */

+  b_y = norm(*(real32_T (*)[3])&x_aposteriori[9]);

 

-  /* 'attitudeKalmanfilter:178' y_n_b=cross(z_n_b,m_n_b); */

-  for (accUpt = 0; accUpt < 3; accUpt++) {

-    z_n_b[accUpt] = -x_aposteriori[accUpt + 3] / y;

-    x_n_b[accUpt] = x_aposteriori[accUpt + 6] / c_y;

+  /* 'attitudeKalmanfilter:169' y_n_b=cross(z_n_b,m_n_b); */

+  for (i = 0; i < 3; i++) {

+    z_n_b[i] = -x_aposteriori[i + 6] / y;

+    x_n_b[i] = x_aposteriori[i + 9] / b_y;

   }

 

   cross(z_n_b, x_n_b, y_n_b);

 

-  /* 'attitudeKalmanfilter:179' y_n_b=y_n_b./norm(y_n_b); */

+  /* 'attitudeKalmanfilter:170' y_n_b=y_n_b./norm(y_n_b); */

   y = norm(y_n_b);

-  for (ib = 0; ib < 3; ib++) {

-    y_n_b[ib] /= y;

+  for (i = 0; i < 3; i++) {

+    y_n_b[i] /= y;

   }

 

-  /* 'attitudeKalmanfilter:181' x_n_b=(cross(y_n_b,z_n_b)); */

+  /* 'attitudeKalmanfilter:172' x_n_b=(cross(y_n_b,z_n_b)); */

   cross(y_n_b, z_n_b, x_n_b);

 

-  /* 'attitudeKalmanfilter:182' x_n_b=x_n_b./norm(x_n_b); */

+  /* 'attitudeKalmanfilter:173' x_n_b=x_n_b./norm(x_n_b); */

   y = norm(x_n_b);

-  for (ib = 0; ib < 3; ib++) {

-    /* 'attitudeKalmanfilter:188' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */

-    Rot_matrix[ib] = x_n_b[ib] / y;

-    Rot_matrix[3 + ib] = y_n_b[ib];

-    Rot_matrix[6 + ib] = z_n_b[ib];

+  for (i = 0; i < 3; i++) {

+    /* 'attitudeKalmanfilter:179' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */

+    Rot_matrix[i] = x_n_b[i] / y;

+    Rot_matrix[3 + i] = y_n_b[i];

+    Rot_matrix[6 + i] = z_n_b[i];

   }

 

-  /* 'attitudeKalmanfilter:192' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */

-  /* 'attitudeKalmanfilter:193' theta=-asin(Rot_matrix(1,3)); */

-  /* 'attitudeKalmanfilter:194' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */

-  /* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */

+  /* 'attitudeKalmanfilter:183' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */

+  /* 'attitudeKalmanfilter:184' theta=-asin(Rot_matrix(1,3)); */

+  /* 'attitudeKalmanfilter:185' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */

+  /* 'attitudeKalmanfilter:186' eulerAngles=[phi;theta;psi]; */

   eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);

   eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);

   eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
index 56f02b4c8..f8f99fa5a 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:43 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

@@ -29,6 +29,6 @@
 /* Variable Definitions */

 

 /* Function Declarations */

-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]);

+extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);

 #endif

 /* End of code generation (attitudeKalmanfilter.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
index b72256a09..689bc49e9 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter_initialize'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
index efb465b25..dcce3cd72 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter_initialize'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
index d0bf625f0..39f8f648c 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter_terminate'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
index 1ad84575f..ea7b9e03e 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter_terminate'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
index bd83cc168..6d5704a7a 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:42 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c
index c88869453..27da4b6b9 100755
--- a/apps/attitude_estimator_ekf/codegen/cross.c
+++ b/apps/attitude_estimator_ekf/codegen/cross.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'cross'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h
index 92e3a884d..8ef2c475c 100755
--- a/apps/attitude_estimator_ekf/codegen/cross.h
+++ b/apps/attitude_estimator_ekf/codegen/cross.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'cross'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c
index 522f6e285..81626589d 100755
--- a/apps/attitude_estimator_ekf/codegen/diag.c
+++ b/apps/attitude_estimator_ekf/codegen/diag.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'diag'

  *

- * C source code generated on: Fri Sep 21 13:56:43 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

@@ -27,7 +27,19 @@
 /*

  *

  */

-void diag(const real32_T v[3], real32_T d[9])

+void b_diag(const real32_T v[9], real32_T d[81])

+{

+  int32_T j;

+  memset((void *)&d[0], 0, 81U * sizeof(real32_T));

+  for (j = 0; j < 9; j++) {

+    d[j + 9 * j] = v[j];

+  }

+}

+

+/*

+ *

+ */

+void c_diag(const real32_T v[3], real32_T d[9])

 {

   int32_T j;

   for (j = 0; j < 9; j++) {

@@ -39,4 +51,28 @@ void diag(const real32_T v[3], real32_T d[9])
   }

 }

 

+/*

+ *

+ */

+void d_diag(const real32_T v[6], real32_T d[36])

+{

+  int32_T j;

+  memset((void *)&d[0], 0, 36U * sizeof(real32_T));

+  for (j = 0; j < 6; j++) {

+    d[j + 6 * j] = v[j];

+  }

+}

+

+/*

+ *

+ */

+void diag(const real32_T v[12], real32_T d[144])

+{

+  int32_T j;

+  memset((void *)&d[0], 0, 144U * sizeof(real32_T));

+  for (j = 0; j < 12; j++) {

+    d[j + 12 * j] = v[j];

+  }

+}

+

 /* End of code generation (diag.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h
index bb92fb242..10cea81b2 100755
--- a/apps/attitude_estimator_ekf/codegen/diag.h
+++ b/apps/attitude_estimator_ekf/codegen/diag.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'diag'

  *

- * C source code generated on: Fri Sep 21 13:56:43 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

@@ -29,6 +29,9 @@
 /* Variable Definitions */

 

 /* Function Declarations */

-extern void diag(const real32_T v[3], real32_T d[9]);

+extern void b_diag(const real32_T v[9], real32_T d[81]);

+extern void c_diag(const real32_T v[3], real32_T d[9]);

+extern void d_diag(const real32_T v[6], real32_T d[36]);

+extern void diag(const real32_T v[12], real32_T d[144]);

 #endif

 /* End of code generation (diag.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c
index e67071dce..2db070e6f 100755
--- a/apps/attitude_estimator_ekf/codegen/eye.c
+++ b/apps/attitude_estimator_ekf/codegen/eye.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'eye'

  *

- * C source code generated on: Fri Sep 21 13:56:43 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

@@ -27,12 +27,12 @@
 /*

  *

  */

-void b_eye(real_T I[81])

+void b_eye(real_T I[144])

 {

   int32_T i;

-  memset((void *)&I[0], 0, 81U * sizeof(real_T));

-  for (i = 0; i < 9; i++) {

-    I[i + 9 * i] = 1.0;

+  memset((void *)&I[0], 0, 144U * sizeof(real_T));

+  for (i = 0; i < 12; i++) {

+    I[i + 12 * i] = 1.0;

   }

 }

 

diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h
index c07a1bc97..027db1c06 100755
--- a/apps/attitude_estimator_ekf/codegen/eye.h
+++ b/apps/attitude_estimator_ekf/codegen/eye.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'eye'

  *

- * C source code generated on: Fri Sep 21 13:56:43 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

@@ -29,7 +29,7 @@
 /* Variable Definitions */

 

 /* Function Declarations */

-extern void b_eye(real_T I[81]);

+extern void b_eye(real_T I[144]);

 extern void eye(real_T I[9]);

 #endif

 /* End of code generation (eye.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/find.c b/apps/attitude_estimator_ekf/codegen/find.c
deleted file mode 100755
index 8f05ef146..000000000
--- a/apps/attitude_estimator_ekf/codegen/find.c
+++ /dev/null
@@ -1,77 +0,0 @@
-/*

- * find.c

- *

- * Code generation for function 'find'

- *

- * C source code generated on: Fri Sep 21 13:56:43 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) */

diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c
index d098162d5..ce29e42cd 100755
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.c
+++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'mrdivide'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

@@ -21,719 +21,345 @@
 /* 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 */

 

 /*

  *

  */

-static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x)

+void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])

 {

-  return 0.0F;

+  int32_T rtemp;

+  int32_T k;

+  real32_T b_A[9];

+  real32_T b_B[36];

+  int32_T r1;

+  int32_T r2;

+  int32_T r3;

+  real32_T maxval;

+  real32_T a21;

+  real32_T Y[36];

+  for (rtemp = 0; rtemp < 3; rtemp++) {

+    for (k = 0; k < 3; k++) {

+      b_A[k + 3 * rtemp] = B[rtemp + 3 * k];

+    }

+  }

+

+  for (rtemp = 0; rtemp < 12; rtemp++) {

+    for (k = 0; k < 3; k++) {

+      b_B[k + 3 * rtemp] = A[rtemp + 12 * k];

+    }

+  }

+

+  r1 = 0;

+  r2 = 1;

+  r3 = 2;

+  maxval = (real32_T)fabsf(b_A[0]);

+  a21 = (real32_T)fabsf(b_A[1]);

+  if (a21 > maxval) {

+    maxval = a21;

+    r1 = 1;

+    r2 = 0;

+  }

+

+  if ((real32_T)fabsf(b_A[2]) > maxval) {

+    r1 = 2;

+    r2 = 1;

+    r3 = 0;

+  }

+

+  b_A[r2] /= b_A[r1];

+  b_A[r3] /= b_A[r1];

+  b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];

+  b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];

+  b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];

+  b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];

+  if ((real32_T)fabsf(b_A[3 + r3]) > (real32_T)fabsf(b_A[3 + r2])) {

+    rtemp = r2;

+    r2 = r3;

+    r3 = rtemp;

+  }

+

+  b_A[3 + r3] /= b_A[3 + r2];

+  b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];

+  for (k = 0; k < 12; k++) {

+    Y[3 * k] = b_B[r1 + 3 * k];

+    Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];

+    Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3

+      + r3];

+    Y[2 + 3 * k] /= b_A[6 + r3];

+    Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];

+    Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];

+    Y[1 + 3 * k] /= b_A[3 + r2];

+    Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];

+    Y[3 * k] /= b_A[r1];

+  }

+

+  for (rtemp = 0; rtemp < 3; rtemp++) {

+    for (k = 0; k < 12; k++) {

+      y[k + 12 * rtemp] = Y[rtemp + 3 * k];

+    }

+  }

 }

 

 /*

  *

  */

-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])

+void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])

 {

-  int32_T loop_ub;

+  int32_T jy;

   int32_T iy;

-  real32_T b_A_data[81];

-  int32_T jA;

-  int32_T tmp_data[9];

-  int32_T ipiv_data[9];

-  int32_T ldap1;

+  real32_T b_A[36];

+  int8_T ipiv[6];

   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 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];

-  }

-

-  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;

+  int32_T loop_ub;

+  real32_T Y[72];

+  for (jy = 0; jy < 6; jy++) {

+    for (iy = 0; iy < 6; iy++) {

+      b_A[iy + 6 * jy] = B[jy + 6 * iy];

     }

 

-    loop_ub = jA - 1;

-    for (iy = 0; iy <= loop_ub; iy++) {

-      ipiv_data[iy] = tmp_data[iy];

-    }

+    ipiv[jy] = (int8_T)(1 + jy);

   }

 

-  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;

-          }

-        }

+  for (j = 0; j < 5; j++) {

+    mmj = -j;

+    jj = j * 7;

+    jp1j = jj + 1;

+    c = mmj + 6;

+    jy = 0;

+    ix = jj;

+    temp = (real32_T)fabsf(b_A[jj]);

+    for (k = 2; k <= c; k++) {

+      ix++;

+      s = (real32_T)fabsf(b_A[ix]);

+      if (s > temp) {

+        jy = k - 1;

+        temp = s;

       }

     }

 

-    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];

+    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 < 6; k++) {

+          temp = b_A[ix];

+          b_A[ix] = b_A[iy];

+          b_A[iy] = temp;

+          ix += 6;

+          iy += 6;

         }

       }

 

-      b_loop_ub = jj + mmj;

-      for (jA = jj + 1; jA + 1 <= b_loop_ub; jA++) {

-        b_A_data[jA] /= b_A_data[jj];

+      loop_ub = (jp1j + mmj) + 5;

+      for (iy = jp1j; iy + 1 <= loop_ub; iy++) {

+        b_A[iy] /= b_A[jj];

       }

     }

 

-    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;

+    c = 5 - j;

+    jy = jj + 6;

+    for (iy = 1; iy <= c; iy++) {

+      if ((real_T)b_A[jy] != 0.0) {

+        temp = b_A[jy] * -1.0F;

+        ix = jp1j;

+        loop_ub = (mmj + jj) + 12;

+        for (k = 7 + jj; k + 1 <= loop_ub; k++) {

+          b_A[k] += b_A[ix] * temp;

           ix++;

         }

       }

 

-      iy += A_sizes[1];

-      jA += A_sizes[1];

+      jy += 6;

+      jj += 6;

     }

   }

 

-  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 (jy = 0; jy < 12; jy++) {

+    for (iy = 0; iy < 6; iy++) {

+      Y[iy + 6 * jy] = A[jy + 12 * iy];

     }

   }

 

-  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 (iy = 0; iy < 6; iy++) {

+    if (ipiv[iy] != iy + 1) {

+      for (j = 0; j < 12; j++) {

+        temp = Y[iy + 6 * j];

+        Y[iy + 6 * j] = Y[(ipiv[iy] + 6 * j) - 1];

+        Y[(ipiv[iy] + 6 * j) - 1] = temp;

       }

     }

   }

 

-  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];

-          }

+  for (j = 0; j < 12; j++) {

+    c = 6 * j;

+    for (k = 0; k < 6; k++) {

+      jy = 6 * k;

+      if ((real_T)Y[k + c] != 0.0) {

+        for (iy = k + 2; iy < 7; iy++) {

+          Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];

         }

       }

     }

   }

-}

 

-/*

- *

- */

-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 (j = 0; j < 12; j++) {

+    c = 6 * j;

+    for (k = 5; k > -1; k += -1) {

+      jy = 6 * k;

+      if ((real_T)Y[k + c] != 0.0) {

+        Y[k + c] /= b_A[k + jy];

+        for (iy = 0; iy + 1 <= k; iy++) {

+          Y[iy + c] -= Y[k + c] * b_A[iy + jy];

         }

-

-        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;

+  for (jy = 0; jy < 6; jy++) {

+    for (iy = 0; iy < 12; iy++) {

+      y[iy + 12 * jy] = Y[jy + 6 * iy];

+    }

+  }

 }

 

 /*

  *

  */

-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])

+void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])

 {

-  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 jy;

+  int32_T iy;

+  real32_T b_A[81];

+  int8_T ipiv[9];

+  int32_T j;

+  int32_T mmj;

+  int32_T jj;

+  int32_T jp1j;

+  int32_T c;

   int32_T ix;

-  real32_T smax;

+  real32_T temp;

+  int32_T k;

   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;

+  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];

     }

 

-    loop_ub = A_sizes[1] - 1;

-    for (k = 0; k <= loop_ub; k++) {

-      jpvt_data[k] = tmp_data[k];

-    }

+    ipiv[jy] = (int8_T)(1 + jy);

   }

 

-  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;

-            }

-          }

-        }

+  for (j = 0; j < 8; j++) {

+    mmj = -j;

+    jj = j * 10;

+    jp1j = jj + 1;

+    c = mmj + 9;

+    jy = 0;

+    ix = jj;

+    temp = (real32_T)fabsf(b_A[jj]);

+    for (k = 2; k <= c; k++) {

+      ix++;

+      s = (real32_T)fabsf(b_A[ix]);

+      if (s > temp) {

+        jy = k - 1;

+        temp = 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++;

+    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;

         }

-

-        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;

+      loop_ub = (jp1j + mmj) + 8;

+      for (iy = jp1j; iy + 1 <= loop_ub; iy++) {

+        b_A[iy] /= b_A[jj];

       }

+    }

 

-      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];

-            }

-          }

+    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;

+          ix++;

         }

-

-        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);

-          }

-        }

-      }

+      jy += 9;

+      jj += 9;

     }

   }

 

-  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 (jy = 0; jy < 12; jy++) {

+    for (iy = 0; iy < 9; iy++) {

+      Y[iy + 9 * jy] = A[jy + 12 * iy];

     }

   }

 

-  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];

+  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;

       }

     }

   }

-}

 

-/*

- *

- */

-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;

+  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];

         }

       }

     }

-

-    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];

+  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];

+        }

+      }

     }

-

-    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];

+  for (jy = 0; jy < 9; jy++) {

+    for (iy = 0; iy < 12; iy++) {

+      y[iy + 12 * jy] = Y[jy + 9 * iy];

     }

   }

 }

diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h
index e807afcc8..b80f34357 100755
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.h
+++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'mrdivide'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

@@ -29,6 +29,8 @@
 /* Variable Definitions */

 

 /* Function Declarations */

-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]);

+extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);

+extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);

+extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);

 #endif

 /* End of code generation (mrdivide.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c
index fa07e1a90..341c93022 100755
--- a/apps/attitude_estimator_ekf/codegen/norm.c
+++ b/apps/attitude_estimator_ekf/codegen/norm.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'norm'

  *

- * C source code generated on: Fri Sep 21 13:56:43 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h
index 63294717f..0f58dbe69 100755
--- a/apps/attitude_estimator_ekf/codegen/norm.h
+++ b/apps/attitude_estimator_ekf/codegen/norm.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'norm'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/power.c b/apps/attitude_estimator_ekf/codegen/power.c
new file mode 100755
index 000000000..8672c7a85
--- /dev/null
+++ b/apps/attitude_estimator_ekf/codegen/power.c
@@ -0,0 +1,84 @@
+/*

+ * power.c

+ *

+ * Code generation for function 'power'

+ *

+ * C source code generated on: Mon Oct 01 19:38:49 2012

+ *

+ */

+

+/* Include files */

+#include "rt_nonfinite.h"

+#include "attitudeKalmanfilter.h"

+#include "power.h"

+

+/* Type Definitions */

+

+/* Named Constants */

+

+/* Variable Declarations */

+

+/* Variable Definitions */

+

+/* Function Declarations */

+static real32_T rt_powf_snf(real32_T u0, real32_T u1);

+

+/* Function Definitions */

+static real32_T rt_powf_snf(real32_T u0, real32_T u1)

+{

+  real32_T y;

+  real32_T f0;

+  real32_T f1;

+  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {

+    y = ((real32_T)rtNaN);

+  } else {

+    f0 = (real32_T)fabsf(u0);

+    f1 = (real32_T)fabsf(u1);

+    if (rtIsInfF(u1)) {

+      if (f0 == 1.0F) {

+        y = ((real32_T)rtNaN);

+      } else if (f0 > 1.0F) {

+        if (u1 > 0.0F) {

+          y = ((real32_T)rtInf);

+        } else {

+          y = 0.0F;

+        }

+      } else if (u1 > 0.0F) {

+        y = 0.0F;

+      } else {

+        y = ((real32_T)rtInf);

+      }

+    } else if (f1 == 0.0F) {

+      y = 1.0F;

+    } else if (f1 == 1.0F) {

+      if (u1 > 0.0F) {

+        y = u0;

+      } else {

+        y = 1.0F / u0;

+      }

+    } else if (u1 == 2.0F) {

+      y = u0 * u0;

+    } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {

+      y = (real32_T)sqrtf(u0);

+    } else if ((u0 < 0.0F) && (u1 > (real32_T)floorf(u1))) {

+      y = ((real32_T)rtNaN);

+    } else {

+      y = (real32_T)powf(u0, u1);

+    }

+  }

+

+  return y;

+}

+

+/*

+ *

+ */

+void power(const real32_T a[12], real_T b, real32_T y[12])

+{

+  int32_T k;

+  for (k = 0; k < 12; k++) {

+    y[k] = rt_powf_snf(a[k], (real32_T)b);

+  }

+}

+

+/* End of code generation (power.c) */

diff --git a/apps/attitude_estimator_ekf/codegen/find.h b/apps/attitude_estimator_ekf/codegen/power.h
index ca525c600..a60f1bb25 100755
--- a/apps/attitude_estimator_ekf/codegen/find.h
+++ b/apps/attitude_estimator_ekf/codegen/power.h
@@ -1,14 +1,14 @@
 /*

- * find.h

+ * power.h

  *

- * Code generation for function 'find'

+ * Code generation for function 'power'

  *

- * C source code generated on: Fri Sep 21 13:56:43 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

-#ifndef __FIND_H__

-#define __FIND_H__

+#ifndef __POWER_H__

+#define __POWER_H__

 /* Include files */

 #include <math.h>

 #include <stdio.h>

@@ -29,6 +29,6 @@
 /* Variable Definitions */

 

 /* Function Declarations */

-extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]);

+extern void power(const real32_T a[12], real_T b, real32_T y[12]);

 #endif

-/* End of code generation (find.h) */

+/* End of code generation (power.h) */

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c
index f1bb71c87..53686acc9 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c
+++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:45 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h
index 6d32d51b5..5ac1dc794 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h
+++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:45 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
index 50581f34d..1e1876b80 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
+++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:45 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
index 12d8734f5..5f1c81f67 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
+++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h
index 419edf01c..5f65f6de9 100755
--- a/apps/attitude_estimator_ekf/codegen/rt_defines.h
+++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
index 42ff21d39..2c687d7a1 100755
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
+++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
index 60128156e..d2ebd0806 100755
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
+++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h
index 2709915e9..b487c8b38 100755
--- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h
+++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h
@@ -3,7 +3,7 @@
  *

  * Code generation for function 'attitudeKalmanfilter'

  *

- * C source code generated on: Fri Sep 21 13:56:44 2012

+ * C source code generated on: Mon Oct 01 19:38:49 2012

  *

  */

 

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 960c5d383..77e4da850 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -949,7 +949,12 @@ int commander_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_create("commander", SCHED_PRIORITY_MAX - 50, 4096, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_spawn("commander",
+					 SCHED_RR,
+					 SCHED_PRIORITY_MAX - 50,
+					 4096,
+					 commander_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
 		exit(0);
 	}
@@ -1007,8 +1012,12 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&current_status, 0, sizeof(current_status));
 	current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
 	current_status.flag_system_armed = false;
+	/* neither manual nor offboard control commands have been received */
 	current_status.offboard_control_signal_found_once = false;
 	current_status.rc_signal_found_once = false;
+	/* mark all signals lost as long as they haven't been found */
+	current_status.rc_signal_lost = true;
+	current_status.offboard_control_signal_lost = true;
 
 	/* advertise to ORB */
 	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/apps/examples/px4_deamon_app/px4_deamon_app.c
index 053163789..6068df5a3 100644
--- a/apps/examples/px4_deamon_app/px4_deamon_app.c
+++ b/apps/examples/px4_deamon_app/px4_deamon_app.c
@@ -91,7 +91,12 @@ int px4_deamon_app_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_create("deamon", SCHED_PRIORITY_DEFAULT, 4096, px4_deamon_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_spawn("deamon",
+					 SCHED_RR,
+					 SCHED_PRIORITY_DEFAULT,
+					 4096,
+					 px4_deamon_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
 		exit(0);
 	}
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
index beabc1142..81ec4ca0b 100644
--- a/apps/fixedwing_control/fixedwing_control.c
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -60,6 +60,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/pid/pid.h>
 #include <systemlib/geo/geo.h>
+#include <systemlib/systemlib.h>
 #include <uORB/topics/debug_key_value.h>
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
@@ -414,7 +415,12 @@ int fixedwing_control_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_create("fixedwing_control", SCHED_PRIORITY_MAX - 20, 4096, fixedwing_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_spawn("fixedwing_control",
+					 SCHED_RR,
+					 SCHED_PRIORITY_MAX - 20,
+					 4096,
+					 fixedwing_control_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
 		exit(0);
 	}
diff --git a/apps/gps/gps.c b/apps/gps/gps.c
index 976beeaab..8304c72e1 100644
--- a/apps/gps/gps.c
+++ b/apps/gps/gps.c
@@ -57,6 +57,8 @@
 #include <v1.0/common/mavlink.h>
 #include <mavlink/mavlink_log.h>
 
+#include <systemlib/systemlib.h>
+
 static bool thread_should_exit;				/**< Deamon status flag */
 static bool thread_running = false;				/**< Deamon status flag */
 static int deamon_task;							/**< Handle of deamon task / thread */
@@ -140,7 +142,12 @@ int gps_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_create("gps", SCHED_PRIORITY_DEFAULT, 4096, gps_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_spawn("gps",
+					 SCHED_RR,
+					 SCHED_PRIORITY_DEFAULT,
+					 4096,
+					 gps_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
 		exit(0);
 	}
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index cf6fb077f..b0cf01fd4 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -75,7 +75,9 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/debug_key_value.h>
+
 #include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
 
 #include "waypoints.h"
 #include "mavlink_log.h"
@@ -203,7 +205,7 @@ int mavlink_missionlib_send_gcs_string(const char *string);
 uint64_t mavlink_missionlib_get_system_timestamp(void);
 
 void handleMessage(mavlink_message_t *msg);
-static void mavlink_update_system();
+static void mavlink_update_system(void);
 
 /**
  * Enable / disable Hardware in the Loop simulation mode.
@@ -1552,9 +1554,9 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 	return uart;
 }
 
-void mavlink_update_system()
+void mavlink_update_system(void)
 {
-	static initialized = false;
+	static bool initialized = false;
 	param_t param_system_id;
 	param_t param_component_id;
 	param_t param_system_type;
@@ -1709,9 +1711,9 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
 		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
 		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
 		/* 2 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
@@ -1869,7 +1871,12 @@ int mavlink_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		mavlink_task = task_spawn("mavlink",
+					  SCHED_RR,
+					  SCHED_PRIORITY_DEFAULT,
+					  6000,
+					  mavlink_thread_main,
+					  (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index ebd9911a3..9acdc6fd3 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -67,6 +67,7 @@
 #include <uORB/topics/actuator_controls.h>
 
 #include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
 
 #include "multirotor_attitude_control.h"
 #include "multirotor_rate_control.h"
@@ -350,7 +351,12 @@ int multirotor_att_control_main(int argc, char *argv[])
 	if (!strcmp(argv[1+optioncount], "start")) {
 
 		thread_should_exit = false;
-		mc_task = task_create("multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL);
+		mc_task = task_spawn("multirotor_att_control",
+				     SCHED_RR,
+				     SCHED_PRIORITY_MAX - 15,
+				     2048,
+				     mc_thread_main,
+				     NULL);
 		exit(0);
 	}
 
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index 48d338c99..eeef2f914 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -66,6 +66,8 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 
+#include <systemlib/systemlib.h>
+
 class FMUServo : public device::CDev
 {
 public:
@@ -169,7 +171,12 @@ FMUServo::init()
 		return ret;
 
 	/* start the IO interface task */
-	_task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr);
+	_task = task_spawn("fmuservo",
+			   SCHED_RR,
+			   SCHED_PRIORITY_DEFAULT,
+			   1024,
+			   (main_t)&FMUServo::task_main_trampoline,
+			   nullptr);
 
 	if (_task < 0) {
 		debug("task start failed: %d", errno);
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index f1c1f9a23..f4af17597 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -61,6 +61,8 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_command.h>
 
+#include <systemlib/systemlib.h>
+
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
 static int deamon_task;				/**< Handle of deamon task / thread */
@@ -120,7 +122,12 @@ int sdlog_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_create("sdlog", SCHED_PRIORITY_DEFAULT - 30, 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_spawn("sdlog",
+					 SCHED_RR,
+					 SCHED_PRIORITY_DEFAULT - 30,
+					 4096,
+					 sdlog_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
 		exit(0);
 	}
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index a68f6166c..b9de42f76 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -1173,11 +1173,12 @@ Sensors::start()
 	ASSERT(_sensors_task == -1);
 
 	/* start the task */
-	_sensors_task = task_create("sensors_task",
-				    SCHED_PRIORITY_MAX - 5,
-				    6000,	/* XXX may be excesssive */
-				    (main_t)&Sensors::task_main_trampoline,
-				    nullptr);
+	_sensors_task = task_spawn("sensors_task",
+				   SCHED_RR,
+				   SCHED_PRIORITY_MAX - 5,
+				   6000,	/* XXX may be excesssive */
+				   (main_t)&Sensors::task_main_trampoline,
+				   nullptr);
 
 	if (_sensors_task < 0) {
 		warn("task start failed");
diff --git a/apps/systemcmds/led/led.c b/apps/systemcmds/led/led.c
index a6d73210a..da0d99964 100644
--- a/apps/systemcmds/led/led.c
+++ b/apps/systemcmds/led/led.c
@@ -53,6 +53,9 @@
 #include <arch/board/up_hrt.h>
 #include <arch/board/drv_led.h>
 
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
 __EXPORT int led_main(int argc, char *argv[]);
 
 
@@ -61,7 +64,7 @@ static bool thread_running = false;	/**< Deamon status flag */
 static int led_task;			/**< Handle of deamon task / thread */
 static int leds;
 
-static int led_init()
+static int led_init(void)
 {
 	leds = open("/dev/led", O_RDONLY | O_NONBLOCK);
 
@@ -76,7 +79,7 @@ static int led_init()
 	return 0;
 }
 
-static void led_deinit()
+static void led_deinit(void)
 {
 	close(leds);
 }
@@ -144,7 +147,12 @@ int led_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		led_task = task_create("led", SCHED_PRIORITY_MAX - 15, 4096, led_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		led_task = task_spawn("led",
+				      SCHED_RR,
+				      SCHED_PRIORITY_MAX - 15,
+				      4096,
+				      led_thread_main,
+				      (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
 		exit(0);
 	}
diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c
index 4c7aae83e..3d46a17a8 100644
--- a/apps/systemlib/systemlib.c
+++ b/apps/systemlib/systemlib.c
@@ -68,10 +68,10 @@ const struct __multiport_info multiport_info = {
  ****************************************************************************/
 
 /****************************************************************************
- * Public Functions
+ * Private Functions
  ****************************************************************************/
 
-void kill_task(FAR _TCB *tcb, FAR void *arg);
+static void kill_task(FAR _TCB *tcb, FAR void *arg);
 
 /****************************************************************************
  * user_start
@@ -116,11 +116,36 @@ void killall()
 	sched_foreach(kill_task, NULL);
 }
 
-void kill_task(FAR _TCB *tcb, FAR void *arg)
+static void kill_task(FAR _TCB *tcb, FAR void *arg)
 {
 	kill(tcb->pid, SIGUSR1);
 }
 
+int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
+{
+	int pid;
+
+	sched_lock();
+
+	/* create the task */
+	pid = task_create(name, priority, stack_size, entry, argv);
+
+	if (pid > 0) {
+		
+		/* configure the scheduler */
+		struct sched_param param;
+
+		param.sched_priority = priority;
+		sched_setscheduler(pid, scheduler, &param);
+
+		/* XXX do any other private task accounting here */
+	}
+
+	sched_unlock();
+
+	return pid;
+}
+
 #define PX4_BOARD_ID_FMU (5)
 
 int fmu_get_board_info(struct fmu_board_info_s *info)
diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h
index add47f440..da1524435 100644
--- a/apps/systemlib/systemlib.h
+++ b/apps/systemlib/systemlib.h
@@ -50,6 +50,14 @@ __EXPORT int reboot(void);
 /** Sends SIGUSR1 to all processes */
 __EXPORT void killall(void);
 
+/** Starts a task and performs any specific accounting, scheduler setup, etc. */
+__EXPORT int task_spawn(const char *name,
+			int priority,
+			int scheduler,
+			int stack_size,
+			main_t entry,
+			const char *argv[]);
+
 enum MULT_PORTS {
 	MULT_0_US2_RXTX = 0,
 	MULT_1_US2_FLOW,