Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

fix line endings, replace CRLF with LF

Browse Source
This commit is contained in:
Felix Ruess
2010-11-08 20:13:21 +00:00
parent 4c0e542054
commit cb8bb6f7fe
36 changed files with 8037 additions and 8037 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TUDelft/Trip50A.xml
+15 -15
View File
@@ -127,29 +127,29 @@
</section>
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="7.6" unit="volt"/>
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="7.6" unit="volt"/>
<!-- Vertical Outerloop
v_ctl_climb_setpoint = ALTITUDE_PGAIN * altitude_error + altitude_pre_climb;
BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
-->
<!-- Vertical Outerloop
v_ctl_climb_setpoint = ALTITUDE_PGAIN * altitude_error + altitude_pre_climb;
BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
-->
<!-- outer loop proportional gain: alt error 5 climb m/s -->
<define name="ALTITUDE_PGAIN" value="-0.0750000029802"/>
<!-- outer loop saturation: m/s-->
<define name="ALTITUDE_MAX_CLIMB" value="5.0"/>
<!-- auto throttle inner loop
float controlled_throttle = v_ctl_auto_throttle_cruise_throttle
+ v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
+ v_ctl_auto_throttle_pgain *
(err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err
+ v_ctl_auto_throttle_dgain * d_err);
/* pitch pre-command */
<!-- auto throttle inner loop
float controlled_throttle = v_ctl_auto_throttle_cruise_throttle
+ v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
+ v_ctl_auto_throttle_pgain *
(err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err
+ v_ctl_auto_throttle_dgain * d_err);
/* pitch pre-command */
float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain)
* v_ctl_auto_throttle_pitch_of_vz_pgain;
* v_ctl_auto_throttle_pitch_of_vz_pgain;
-->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.40000000596"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.20"/>
conf/airframes/TUDelft/Trip50B.xml
+15 -15
View File
@@ -132,29 +132,29 @@
</section>
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="7.6" unit="volt"/>
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="7.6" unit="volt"/>
<!-- Vertical Outerloop
v_ctl_climb_setpoint = ALTITUDE_PGAIN * altitude_error + altitude_pre_climb;
BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
-->
<!-- Vertical Outerloop
v_ctl_climb_setpoint = ALTITUDE_PGAIN * altitude_error + altitude_pre_climb;
BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
-->
<!-- outer loop proportional gain: alt error 5 climb m/s -->
<define name="ALTITUDE_PGAIN" value="-0.0750000029802"/>
<!-- outer loop saturation: m/s-->
<define name="ALTITUDE_MAX_CLIMB" value="5.0"/>
<!-- auto throttle inner loop
float controlled_throttle = v_ctl_auto_throttle_cruise_throttle
+ v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
+ v_ctl_auto_throttle_pgain *
(err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err
+ v_ctl_auto_throttle_dgain * d_err);
/* pitch pre-command */
<!-- auto throttle inner loop
float controlled_throttle = v_ctl_auto_throttle_cruise_throttle
+ v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
+ v_ctl_auto_throttle_pgain *
(err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err
+ v_ctl_auto_throttle_dgain * d_err);
/* pitch pre-command */
float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain)
* v_ctl_auto_throttle_pitch_of_vz_pgain;
* v_ctl_auto_throttle_pitch_of_vz_pgain;
-->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.40000000596"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.20"/>
conf/radios/aron.xml
+13 -13
View File
@@ -1,13 +1,13 @@
<!DOCTYPE radio SYSTEM "radio.dtd">
<radio name="T9cap" data_min="900" data_max="2100" sync_min="5000" sync_max="15000" pulse_type="POSITIVE">
<channel ctl="switch_E" function="MODE" min="966" neutral="1526" max="2079" average="10"/>
<channel ctl="switch_G" function="DISABLE" min="2080" neutral="1525" max="969" average="10"/>
<channel ctl="left_stick_horiz" function="YAW" min="1115" neutral="1520" max="1940" average="0"/>
<channel ctl="right_stick_vert" function="PITCH" min="1937" neutral="1518" max="1112" average="0"/>
<channel ctl="right_stick_horiz" function="ROLL" min="1114" neutral="1525" max="1939" average="0"/>
<channel ctl="left_stick_vert" function="THROTTLE" min="1115" neutral="1115" max="1940" average="0"/>
</radio>
<!DOCTYPE radio SYSTEM "radio.dtd">
<radio name="T9cap" data_min="900" data_max="2100" sync_min="5000" sync_max="15000" pulse_type="POSITIVE">
<channel ctl="switch_E" function="MODE" min="966" neutral="1526" max="2079" average="10"/>
<channel ctl="switch_G" function="DISABLE" min="2080" neutral="1525" max="969" average="10"/>
<channel ctl="left_stick_horiz" function="YAW" min="1115" neutral="1520" max="1940" average="0"/>
<channel ctl="right_stick_vert" function="PITCH" min="1937" neutral="1518" max="1112" average="0"/>
<channel ctl="right_stick_horiz" function="ROLL" min="1114" neutral="1525" max="1939" average="0"/>
<channel ctl="left_stick_vert" function="THROTTLE" min="1115" neutral="1115" max="1940" average="0"/>
</radio>
conf/settings/settings_beth_twist_old.xml
+3 -3
View File
@@ -37,9 +37,9 @@
<dl_setting var="controller.c" min="0.001" step="0.01" max="10" module="beth/overo_twist_controller" shortname="c">
</dl_setting>
<!--float ulim; 1
float Vm; .1
float VM; 300
<!--float ulim; 1
float Vm; .1
float VM; 300
float satval1; 0.4
float satval2; 1
const float c;.04-->
conf/simulator/Malolo1/Malolo1-set.xml
+103 -103
View File
@@ -1,103 +1,103 @@
<?xml version="1.0"?>
<!--
************************************************************************
Rascal 110 R/C airplane config. This files ties together all the components
used by FGFS to represent the Rascal 110 (by Sig Mfg) including the flight
dynamics model, and external 3D model.
************************************************************************
-->
<PropertyList>
<sim>
<description>Malolo1(R/C)</description>
<author>Innis Cunningham, Josh Wilson</author>
<aircraft-version>0.0</aircraft-version>
<startup>
<splash-texture>Aircraft/Malolo1/Malolo1-splash.rgb</splash-texture>
</startup>
<flight-model>jsb</flight-model>
<aero>Malolo1</aero>
<fuel-fraction>0.8</fuel-fraction>
<!--
<systems>
<autopilot>
<path>Aircraft/Malolo1/Systems/110-autopilot.xml</path>
</autopilot>
<electrical>
<path>Aircraft/Malolo1/Systems/electrical.xml</path>
</electrical>
</systems> -->
<sound>
<path>Aircraft/Generic/generic-sound.xml</path>
</sound>
<panel>
<visibility archive="n">false</visibility>
</panel>
<model>
<path archive="y">Aircraft/Malolo1/Models/Malolo1.xml</path>
</model>
<view>
<internal archive="y">true</internal>
<config>
<x-offset-m archive="y">0.0</x-offset-m>
<y-offset-m archive="y">0.26</y-offset-m>
<z-offset-m archive="y">0.34</z-offset-m>
<pitch-offset-deg>-8</pitch-offset-deg>
</config>
</view>
<chase-distance-m archive="y" type="double">-15.0</chase-distance-m>
<help>
<title>YardStik 110 (Sig Mfg)</title>
<line>Cruise speed: 60 mph</line>
<line>Never-exceed (Vne): 85 mph</line>
<line>Best Glide (Vglide): 20 mph</line>
<line>Maneuvering (Va): 50 mph</line>
<line>Approach speed: 15-25 mph</line>
<line>Stall speed (Vs): 10 mph</line>
</help>
</sim>
<controls>
<flight>
<aileron-trim>-0.01</aileron-trim> <!-- fixed -->
<elevator-trim>0.00</elevator-trim> <!-- controllable -->
<rudder-trim>0.00</rudder-trim> <!-- fixed -->
</flight>
<engines>
<engine n="0">
<magnetos>3</magnetos>
</engine>
</engines>
<door>1.0</door>
</controls>
<engines>
<engine>
<rpm type="double">700</rpm>
</engine>
</engines>
<!-- An autopilot on a Cub??? -->
<autopilot>
<config>
<min-climb-speed-kt type="float">48.0</min-climb-speed-kt>
<best-climb-speed-kt type="float">56.0</best-climb-speed-kt>
<target-climb-rate-fpm type="float">400.0</target-climb-rate-fpm>
<target-descent-rate-fpm type="float">1000.0</target-descent-rate-fpm>
<elevator-adj-factor type="float">6000.0</elevator-adj-factor>
<integral-contribution type="float">0.008</integral-contribution>
<zero-pitch-throttle type="float">0.35</zero-pitch-throttle>
<zero-pitch-trim-full-throttle type="float">0.001</zero-pitch-trim-full-throttle>
</config>
</autopilot>
</PropertyList>
<?xml version="1.0"?>
<!--
************************************************************************
Rascal 110 R/C airplane config. This files ties together all the components
used by FGFS to represent the Rascal 110 (by Sig Mfg) including the flight
dynamics model, and external 3D model.
************************************************************************
-->
<PropertyList>
<sim>
<description>Malolo1(R/C)</description>
<author>Innis Cunningham, Josh Wilson</author>
<aircraft-version>0.0</aircraft-version>
<startup>
<splash-texture>Aircraft/Malolo1/Malolo1-splash.rgb</splash-texture>
</startup>
<flight-model>jsb</flight-model>
<aero>Malolo1</aero>
<fuel-fraction>0.8</fuel-fraction>
<!--
<systems>
<autopilot>
<path>Aircraft/Malolo1/Systems/110-autopilot.xml</path>
</autopilot>
<electrical>
<path>Aircraft/Malolo1/Systems/electrical.xml</path>
</electrical>
</systems> -->
<sound>
<path>Aircraft/Generic/generic-sound.xml</path>
</sound>
<panel>
<visibility archive="n">false</visibility>
</panel>
<model>
<path archive="y">Aircraft/Malolo1/Models/Malolo1.xml</path>
</model>
<view>
<internal archive="y">true</internal>
<config>
<x-offset-m archive="y">0.0</x-offset-m>
<y-offset-m archive="y">0.26</y-offset-m>
<z-offset-m archive="y">0.34</z-offset-m>
<pitch-offset-deg>-8</pitch-offset-deg>
</config>
</view>
<chase-distance-m archive="y" type="double">-15.0</chase-distance-m>
<help>
<title>YardStik 110 (Sig Mfg)</title>
<line>Cruise speed: 60 mph</line>
<line>Never-exceed (Vne): 85 mph</line>
<line>Best Glide (Vglide): 20 mph</line>
<line>Maneuvering (Va): 50 mph</line>
<line>Approach speed: 15-25 mph</line>
<line>Stall speed (Vs): 10 mph</line>
</help>
</sim>
<controls>
<flight>
<aileron-trim>-0.01</aileron-trim> <!-- fixed -->
<elevator-trim>0.00</elevator-trim> <!-- controllable -->
<rudder-trim>0.00</rudder-trim> <!-- fixed -->
</flight>
<engines>
<engine n="0">
<magnetos>3</magnetos>
</engine>
</engines>
<door>1.0</door>
</controls>
<engines>
<engine>
<rpm type="double">700</rpm>
</engine>
</engines>
<!-- An autopilot on a Cub??? -->
<autopilot>
<config>
<min-climb-speed-kt type="float">48.0</min-climb-speed-kt>
<best-climb-speed-kt type="float">56.0</best-climb-speed-kt>
<target-climb-rate-fpm type="float">400.0</target-climb-rate-fpm>
<target-descent-rate-fpm type="float">1000.0</target-descent-rate-fpm>
<elevator-adj-factor type="float">6000.0</elevator-adj-factor>
<integral-contribution type="float">0.008</integral-contribution>
<zero-pitch-throttle type="float">0.35</zero-pitch-throttle>
<zero-pitch-trim-full-throttle type="float">0.001</zero-pitch-trim-full-throttle>
</config>
</autopilot>
</PropertyList>
conf/simulator/Malolo1/Malolo1.xml
+546 -546
View File
File diff suppressed because it is too large Load Diff
conf/simulator/Malolo1/Models/Malolo1.ac
+4197 -4197
View File
File diff suppressed because it is too large Load Diff
conf/simulator/Malolo1/Models/Malolo1.xml
+103 -103
View File
@@ -1,103 +1,103 @@
<?xml version="1.0"?>
<PropertyList>
<path>Malolo1.ac</path>
<offsets>
<z-m>-0.01</z-m>
<pitch-deg>0.2</pitch-deg>
</offsets>
<animation>
<type>rotate</type>
<object-name>rhelevon</object-name>
<property>controls/flight/elevator</property>
<factor>15</factor>
<offset-deg>0</offset-deg>
<center>
<x-m>0.65</x-m>
<y-m>1.4</y-m>
<z-m>0.07</z-m>
</center>
<axis>
<x>0.215</x>
<y>1</y>
<z>0.056</z>
</axis>
</animation>
<animation>
<type>rotate</type>
<object-name>rhelevon</object-name>
<property>controls/flight/aileron</property>
<factor>-15</factor>
<offset-deg>0</offset-deg>
<center>
<x-m>0.65</x-m>
<y-m>1.4</y-m>
<z-m>0.07</z-m>
</center>
<axis>
<x>0.215</x>
<y>1</y>
<z>0.056</z>
</axis>
</animation>
<animation>
<type>rotate</type>
<object-name>lhelevon</object-name>
<property>controls/flight/elevator</property>
<factor>15</factor>
<offset-deg>0</offset-deg>
<center>
<x-m>0.65</x-m>
<y-m>-1.4</y-m>
<z-m>0.07</z-m>
</center>
<axis>
<x>-0.215</x>
<y>1</y>
<z>-0.056</z>
</axis>
</animation>
<animation>
<type>rotate</type>
<object-name>lhelevon</object-name>
<property>controls/flight/aileron</property>
<factor>15</factor>
<offset-deg>0</offset-deg>
<center>
<x-m>0.65</x-m>
<y-m>-1.4</y-m>
<z-m>0.07</z-m>
</center>
<axis>
<x>-0.215</x>
<y>1</y>
<z>-0.056</z>
</axis>
</animation>
<animation>
<type>spin</type>
<object-name>prop2</object-name>
<property>engines/engine/rpm</property>
<factor>-0.2</factor>
<center>
<x-m>0</x-m>
<y-m>0</y-m>
<z-m>0.0156</z-m>
</center>
<axis>
<x>1</x>
<y>0</y>
<z>0</z>
</axis>
</animation>
</PropertyList>
<?xml version="1.0"?>
<PropertyList>
<path>Malolo1.ac</path>
<offsets>
<z-m>-0.01</z-m>
<pitch-deg>0.2</pitch-deg>
</offsets>
<animation>
<type>rotate</type>
<object-name>rhelevon</object-name>
<property>controls/flight/elevator</property>
<factor>15</factor>
<offset-deg>0</offset-deg>
<center>
<x-m>0.65</x-m>
<y-m>1.4</y-m>
<z-m>0.07</z-m>
</center>
<axis>
<x>0.215</x>
<y>1</y>
<z>0.056</z>
</axis>
</animation>
<animation>
<type>rotate</type>
<object-name>rhelevon</object-name>
<property>controls/flight/aileron</property>
<factor>-15</factor>
<offset-deg>0</offset-deg>
<center>
<x-m>0.65</x-m>
<y-m>1.4</y-m>
<z-m>0.07</z-m>
</center>
<axis>
<x>0.215</x>
<y>1</y>
<z>0.056</z>
</axis>
</animation>
<animation>
<type>rotate</type>
<object-name>lhelevon</object-name>
<property>controls/flight/elevator</property>
<factor>15</factor>
<offset-deg>0</offset-deg>
<center>
<x-m>0.65</x-m>
<y-m>-1.4</y-m>
<z-m>0.07</z-m>
</center>
<axis>
<x>-0.215</x>
<y>1</y>
<z>-0.056</z>
</axis>
</animation>
<animation>
<type>rotate</type>
<object-name>lhelevon</object-name>
<property>controls/flight/aileron</property>
<factor>15</factor>
<offset-deg>0</offset-deg>
<center>
<x-m>0.65</x-m>
<y-m>-1.4</y-m>
<z-m>0.07</z-m>
</center>
<axis>
<x>-0.215</x>
<y>1</y>
<z>-0.056</z>
</axis>
</animation>
<animation>
<type>spin</type>
<object-name>prop2</object-name>
<property>engines/engine/rpm</property>
<factor>-0.2</factor>
<center>
<x-m>0</x-m>
<y-m>0</y-m>
<z-m>0.0156</z-m>
</center>
<axis>
<x>1</x>
<y>0</y>
<z>0</z>
</axis>
</animation>
</PropertyList>
conf/system/udev/rules/1-jtag-rule.rules
+4 -4
View File
@@ -1,4 +1,4 @@
SUBSYSTEM=="usb", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6010",
MODE="0666", GROUP="plugdev"
in the udev rules
SUBSYSTEM=="usb", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6010",
MODE="0666", GROUP="plugdev"
in the udev rules
sw/airborne/arch/lpc21/efsl/conf/config.h
+1 -1
View File
@@ -15,7 +15,7 @@
*/
/*#define HW_ENDPOINT_LINUX*/
/*#define HW_ENDPOINT_ATMEGA128_SD*/
/*#define HW_ENDPOINT_ATMEGA128_SD*/
#define HW_ENDPOINT_LPC2000_SD
sw/airborne/arch/lpc21/efsl/inc/interfaces/LPC2000_regs.h
+72 -72
View File
@@ -1,72 +1,72 @@
/* based on LPC213x.h from Keil GmbH (keil.com/arm.com) */
#ifndef LPC2000_regs_h
#define LPC2000_regs_h
/* Pin Connect Block */
#define PINSEL0 (*((volatile unsigned long *) 0xE002C000))
#define PINSEL1 (*((volatile unsigned long *) 0xE002C004))
#define PINSEL2 (*((volatile unsigned long *) 0xE002C014))
/* General Purpose Input/Output (GPIO) */
#define IOPIN0 (*((volatile unsigned long *) 0xE0028000))
#define IOSET0 (*((volatile unsigned long *) 0xE0028004))
#define IODIR0 (*((volatile unsigned long *) 0xE0028008))
#define IOCLR0 (*((volatile unsigned long *) 0xE002800C))
#define IOPIN1 (*((volatile unsigned long *) 0xE0028010))
#define IOSET1 (*((volatile unsigned long *) 0xE0028014))
#define IODIR1 (*((volatile unsigned long *) 0xE0028018))
#define IOCLR1 (*((volatile unsigned long *) 0xE002801C))
/* SPI0 (Serial Peripheral Interface 0) */
#define S0SPCR (*((volatile unsigned char *) 0xE0020000))
#define S0SPSR (*((volatile unsigned char *) 0xE0020004))
#define S0SPDR (*((volatile unsigned char *) 0xE0020008))
#define S0SPCCR (*((volatile unsigned char *) 0xE002000C))
#define S0SPTCR (*((volatile unsigned char *) 0xE0020010))
#define S0SPTSR (*((volatile unsigned char *) 0xE0020014))
#define S0SPTOR (*((volatile unsigned char *) 0xE0020018))
#define S0SPINT (*((volatile unsigned char *) 0xE002001C))
/* SSP Controller */
#define SSPCR0 (*((volatile unsigned short* ) 0xE0068000))
#define SSPCR1 (*((volatile unsigned char * ) 0xE0068004))
#define SSPDR (*((volatile unsigned short* ) 0xE0068008))
#define SSPSR (*((volatile unsigned char * ) 0xE006800C))
#define SSPCPSR (*((volatile unsigned char * ) 0xE0068010))
#define SSPIMSC (*((volatile unsigned char * ) 0xE0068014))
#define SSPRIS (*((volatile unsigned char * ) 0xE0068018))
#define SSPMIS (*((volatile unsigned char * ) 0xE006801C))
#define SSPICR (*((volatile unsigned char * ) 0xE0068020))
#define SSPDMACR (*((volatile unsigned char * ) 0xE0068024))
/* Real Time Clock */
/* maybe useful for the efsl time-handling : */
#define ILR (*((volatile unsigned char *) 0xE0024000))
#define CTC (*((volatile unsigned short*) 0xE0024004))
#define CCR (*((volatile unsigned char *) 0xE0024008))
#define CIIR (*((volatile unsigned char *) 0xE002400C))
#define AMR (*((volatile unsigned char *) 0xE0024010))
#define CTIME0 (*((volatile unsigned long *) 0xE0024014))
#define CTIME1 (*((volatile unsigned long *) 0xE0024018))
#define CTIME2 (*((volatile unsigned long *) 0xE002401C))
#define SEC (*((volatile unsigned char *) 0xE0024020))
#define MIN (*((volatile unsigned char *) 0xE0024024))
#define HOUR (*((volatile unsigned char *) 0xE0024028))
#define DOM (*((volatile unsigned char *) 0xE002402C))
#define DOW (*((volatile unsigned char *) 0xE0024030))
#define DOY (*((volatile unsigned short*) 0xE0024034))
#define MONTH (*((volatile unsigned char *) 0xE0024038))
#define YEAR (*((volatile unsigned short*) 0xE002403C))
#define ALSEC (*((volatile unsigned char *) 0xE0024060))
#define ALMIN (*((volatile unsigned char *) 0xE0024064))
#define ALHOUR (*((volatile unsigned char *) 0xE0024068))
#define ALDOM (*((volatile unsigned char *) 0xE002406C))
#define ALDOW (*((volatile unsigned char *) 0xE0024070))
#define ALDOY (*((volatile unsigned short*) 0xE0024074))
#define ALMON (*((volatile unsigned char *) 0xE0024078))
#define ALYEAR (*((volatile unsigned short*) 0xE002407C))
#define PREINT (*((volatile unsigned short*) 0xE0024080))
#define PREFRAC (*((volatile unsigned short*) 0xE0024084))
#endif
/* based on LPC213x.h from Keil GmbH (keil.com/arm.com) */
#ifndef LPC2000_regs_h
#define LPC2000_regs_h
/* Pin Connect Block */
#define PINSEL0 (*((volatile unsigned long *) 0xE002C000))
#define PINSEL1 (*((volatile unsigned long *) 0xE002C004))
#define PINSEL2 (*((volatile unsigned long *) 0xE002C014))
/* General Purpose Input/Output (GPIO) */
#define IOPIN0 (*((volatile unsigned long *) 0xE0028000))
#define IOSET0 (*((volatile unsigned long *) 0xE0028004))
#define IODIR0 (*((volatile unsigned long *) 0xE0028008))
#define IOCLR0 (*((volatile unsigned long *) 0xE002800C))
#define IOPIN1 (*((volatile unsigned long *) 0xE0028010))
#define IOSET1 (*((volatile unsigned long *) 0xE0028014))
#define IODIR1 (*((volatile unsigned long *) 0xE0028018))
#define IOCLR1 (*((volatile unsigned long *) 0xE002801C))
/* SPI0 (Serial Peripheral Interface 0) */
#define S0SPCR (*((volatile unsigned char *) 0xE0020000))
#define S0SPSR (*((volatile unsigned char *) 0xE0020004))
#define S0SPDR (*((volatile unsigned char *) 0xE0020008))
#define S0SPCCR (*((volatile unsigned char *) 0xE002000C))
#define S0SPTCR (*((volatile unsigned char *) 0xE0020010))
#define S0SPTSR (*((volatile unsigned char *) 0xE0020014))
#define S0SPTOR (*((volatile unsigned char *) 0xE0020018))
#define S0SPINT (*((volatile unsigned char *) 0xE002001C))
/* SSP Controller */
#define SSPCR0 (*((volatile unsigned short* ) 0xE0068000))
#define SSPCR1 (*((volatile unsigned char * ) 0xE0068004))
#define SSPDR (*((volatile unsigned short* ) 0xE0068008))
#define SSPSR (*((volatile unsigned char * ) 0xE006800C))
#define SSPCPSR (*((volatile unsigned char * ) 0xE0068010))
#define SSPIMSC (*((volatile unsigned char * ) 0xE0068014))
#define SSPRIS (*((volatile unsigned char * ) 0xE0068018))
#define SSPMIS (*((volatile unsigned char * ) 0xE006801C))
#define SSPICR (*((volatile unsigned char * ) 0xE0068020))
#define SSPDMACR (*((volatile unsigned char * ) 0xE0068024))
/* Real Time Clock */
/* maybe useful for the efsl time-handling : */
#define ILR (*((volatile unsigned char *) 0xE0024000))
#define CTC (*((volatile unsigned short*) 0xE0024004))
#define CCR (*((volatile unsigned char *) 0xE0024008))
#define CIIR (*((volatile unsigned char *) 0xE002400C))
#define AMR (*((volatile unsigned char *) 0xE0024010))
#define CTIME0 (*((volatile unsigned long *) 0xE0024014))
#define CTIME1 (*((volatile unsigned long *) 0xE0024018))
#define CTIME2 (*((volatile unsigned long *) 0xE002401C))
#define SEC (*((volatile unsigned char *) 0xE0024020))
#define MIN (*((volatile unsigned char *) 0xE0024024))
#define HOUR (*((volatile unsigned char *) 0xE0024028))
#define DOM (*((volatile unsigned char *) 0xE002402C))
#define DOW (*((volatile unsigned char *) 0xE0024030))
#define DOY (*((volatile unsigned short*) 0xE0024034))
#define MONTH (*((volatile unsigned char *) 0xE0024038))
#define YEAR (*((volatile unsigned short*) 0xE002403C))
#define ALSEC (*((volatile unsigned char *) 0xE0024060))
#define ALMIN (*((volatile unsigned char *) 0xE0024064))
#define ALHOUR (*((volatile unsigned char *) 0xE0024068))
#define ALDOM (*((volatile unsigned char *) 0xE002406C))
#define ALDOW (*((volatile unsigned char *) 0xE0024070))
#define ALDOY (*((volatile unsigned short*) 0xE0024074))
#define ALMON (*((volatile unsigned char *) 0xE0024078))
#define ALYEAR (*((volatile unsigned short*) 0xE002407C))
#define PREINT (*((volatile unsigned short*) 0xE0024080))
#define PREFRAC (*((volatile unsigned short*) 0xE0024084))
#endif
sw/airborne/arch/lpc21/efsl/inc/interfaces/sd.h
+66 -66
View File
@@ -1,66 +1,66 @@
/*****************************************************************************\
* efs - General purpose Embedded Filesystem library *
* --------------------------------------------------------- *
* *
* Filename : sd.h *
* Revision : Initial developement *
* Description : Headerfile for sd.c *
* *
* This program is free software; you can redistribute it and/or *
* modify it under the terms of the GNU General Public License *
* as published by the Free Software Foundation; version 2 *
* of the License. *
*
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* As a special exception, if other files instantiate templates or *
* use macros or inline functions from this file, or you compile this *
* file and link it with other works to produce a work based on this file, *
* this file does not by itself cause the resulting work to be covered *
* by the GNU General Public License. However the source code for this *
* file must still be made available in accordance with section (3) of *
* the GNU General Public License. *
* *
* This exception does not invalidate any other reasons why a work based *
* on this file might be covered by the GNU General Public License. *
* *
* (c)2006 Lennart Yseboodt *
* (c)2006 Michael De Nil *
\*****************************************************************************/
#ifndef __SD_H_
#define __SD_H_
#include "config.h"
#include "types.h"
#include "../debug.h"
#ifdef HW_ENDPOINT_ATMEGA128_SD
#include "interfaces/atmega128.h"
#endif
#ifdef HW_ENDPOINT_DSP_TI6713_SD
#include "interfaces/dsp67xx.h"
#endif
#ifdef HW_ENDPOINT_LPC2000_SD
#include "interfaces/lpc2000_spi.h"
#endif
#define CMDREAD 17
#define CMDWRITE 24
#define CMDREADCSD 9
esint8 sd_Init(hwInterface *iface);
void sd_Command(hwInterface *iface,euint8 cmd, euint16 paramx, euint16 paramy);
euint8 sd_Resp8b(hwInterface *iface);
void sd_Resp8bError(hwInterface *iface,euint8 value);
euint16 sd_Resp16b(hwInterface *iface);
esint8 sd_State(hwInterface *iface);
esint8 sd_readSector(hwInterface *iface,euint32 address,euint8* buf, euint16 len);
esint8 sd_writeSector(hwInterface *iface,euint32 address, euint8* buf);
esint8 sd_getDriveSize(hwInterface *iface, euint32* drive_size );
#endif
/*****************************************************************************\
* efs - General purpose Embedded Filesystem library *
* --------------------------------------------------------- *
* *
* Filename : sd.h *
* Revision : Initial developement *
* Description : Headerfile for sd.c *
* *
* This program is free software; you can redistribute it and/or *
* modify it under the terms of the GNU General Public License *
* as published by the Free Software Foundation; version 2 *
* of the License. *
*
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* As a special exception, if other files instantiate templates or *
* use macros or inline functions from this file, or you compile this *
* file and link it with other works to produce a work based on this file, *
* this file does not by itself cause the resulting work to be covered *
* by the GNU General Public License. However the source code for this *
* file must still be made available in accordance with section (3) of *
* the GNU General Public License. *
* *
* This exception does not invalidate any other reasons why a work based *
* on this file might be covered by the GNU General Public License. *
* *
* (c)2006 Lennart Yseboodt *
* (c)2006 Michael De Nil *
\*****************************************************************************/
#ifndef __SD_H_
#define __SD_H_
#include "config.h"
#include "types.h"
#include "../debug.h"
#ifdef HW_ENDPOINT_ATMEGA128_SD
#include "interfaces/atmega128.h"
#endif
#ifdef HW_ENDPOINT_DSP_TI6713_SD
#include "interfaces/dsp67xx.h"
#endif
#ifdef HW_ENDPOINT_LPC2000_SD
#include "interfaces/lpc2000_spi.h"
#endif
#define CMDREAD 17
#define CMDWRITE 24
#define CMDREADCSD 9
esint8 sd_Init(hwInterface *iface);
void sd_Command(hwInterface *iface,euint8 cmd, euint16 paramx, euint16 paramy);
euint8 sd_Resp8b(hwInterface *iface);
void sd_Resp8bError(hwInterface *iface,euint8 value);
euint16 sd_Resp16b(hwInterface *iface);
esint8 sd_State(hwInterface *iface);
esint8 sd_readSector(hwInterface *iface,euint32 address,euint8* buf, euint16 len);
esint8 sd_writeSector(hwInterface *iface,euint32 address, euint8* buf);
esint8 sd_getDriveSize(hwInterface *iface, euint32* drive_size );
#endif
sw/airborne/arch/lpc21/efsl/src/interfaces/sd.c
+348 -348
View File
File diff suppressed because it is too large Load Diff
sw/airborne/firmwares/helper/arduimu_Firmware_WithGps/arduimu/ADC.pde
+1 -1
View File
@@ -92,4 +92,4 @@ ISR(ADC_vect)
ADMUX = (analog_reference << 6) | MuxSel;
// start the conversion
ADCSRA|= (1<<ADSC);
}
}
sw/airborne/firmwares/helper/arduimu_Firmware_WithGps/arduimu/Compass.pde
+1 -1
View File
@@ -71,4 +71,4 @@ void Compass_Heading()
MAG_Y = magnetom_y*cos_roll-magnetom_z*sin_roll;
// Magnetic Heading
MAG_Heading = atan2(-MAG_Y,MAG_X);
}
}
sw/airborne/firmwares/helper/arduimu_Firmware_WithGps/arduimu/DCM.pde
+1 -1
View File
@@ -277,4 +277,4 @@ void Euler_angles(void)
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
#endif
}
sw/airborne/firmwares/helper/arduimu_Firmware_WithGps/arduimu/GPS_EM406.pde
+1 -1
View File
@@ -201,4 +201,4 @@ Serial.begin(38400); //Universal Synchronous Asynchronous Recieving Transmiting
}
#endif
sw/airborne/firmwares/helper/arduimu_Firmware_WithGps/arduimu/GPS_NMEA.pde
+1 -1
View File
@@ -289,4 +289,4 @@ void decode_gps(void)
}
}
#endif
sw/airborne/firmwares/helper/arduimu_Firmware_WithGps/arduimu/Output.pde
+1 -1
View File
@@ -408,4 +408,4 @@ long convert_to_dec(float x)
{
return x*10000000;
}
sw/airborne/firmwares/helper/arduimu_Firmware_WithGps/arduimu/Vector.pde
+1 -1
View File
@@ -37,4 +37,4 @@ void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
}
sw/airborne/firmwares/helper/arduimu_Firmware_WithGps/arduimu/arduimu.pde
+1 -1
View File
@@ -727,4 +727,4 @@ EEPROM memory map
21 0x15 ..
22 0x16 Ground Altitude
23 0x17 ..
*/
*/
sw/airborne/firmwares/helper/arduimu_Firmware_WithGps/arduimu/matrix.pde
+1 -1
View File
@@ -19,4 +19,4 @@ void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
}
}
sw/airborne/firmwares/helper/arduimu_Firmware_WithGps/arduimu/press_alt.pde
+1 -1
View File
@@ -241,4 +241,4 @@ void TWIdocmd(unsigned char *msg)
STOP;
}
#endif
#endif
sw/airborne/fms/libeknav/assertions.hpp
+65 -65
View File
@@ -1,65 +1,65 @@
/*
* assertions.hpp
*
* Created on: Aug 6, 2009
* Author: Jonathan Brandmeyer
*
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*
*/
#ifndef AHRS_ASSERTIONS_HPP
#define AHRS_ASSERTIONS_HPP
#include <Eigen/Core>
#include <cmath>
template<typename MatrixBase>
bool hasNaN(const MatrixBase& expr);
template<typename MatrixBase>
bool hasInf(const MatrixBase& expr);
template<typename MatrixBase>
bool hasNaN(const MatrixBase& expr)
{
for (int j = 0; j != expr.cols(); ++j) {
for (int i = 0; i != expr.rows(); ++i) {
if (std::isnan(expr.coeff(i, j)))
return true;
}
}
return false;
}
template<typename MatrixBase>
bool hasInf(const MatrixBase& expr)
{
for (int i = 0; i != expr.cols(); ++i) {
for (int j = 0; j != expr.rows(); ++j) {
if (std::isinf(expr.coeff(j, i)))
return true;
}
}
return false;
}
template<typename MatrixBase>
bool isReal(const MatrixBase& exp)
{
return !hasNaN(exp) && ! hasInf(exp);
}
#endif /* ASSERTIONS_HPP_ */
/*
* assertions.hpp
*
* Created on: Aug 6, 2009
* Author: Jonathan Brandmeyer
*
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*
*/
#ifndef AHRS_ASSERTIONS_HPP
#define AHRS_ASSERTIONS_HPP
#include <Eigen/Core>
#include <cmath>
template<typename MatrixBase>
bool hasNaN(const MatrixBase& expr);
template<typename MatrixBase>
bool hasInf(const MatrixBase& expr);
template<typename MatrixBase>
bool hasNaN(const MatrixBase& expr)
{
for (int j = 0; j != expr.cols(); ++j) {
for (int i = 0; i != expr.rows(); ++i) {
if (std::isnan(expr.coeff(i, j)))
return true;
}
}
return false;
}
template<typename MatrixBase>
bool hasInf(const MatrixBase& expr)
{
for (int i = 0; i != expr.cols(); ++i) {
for (int j = 0; j != expr.rows(); ++j) {
if (std::isinf(expr.coeff(j, i)))
return true;
}
}
return false;
}
template<typename MatrixBase>
bool isReal(const MatrixBase& exp)
{
return !hasNaN(exp) && ! hasInf(exp);
}
#endif /* ASSERTIONS_HPP_ */
sw/airborne/fms/libeknav/basic_ins_qkf.cpp
+154 -154
View File
@@ -1,154 +1,154 @@
/*
* basic_ins_qkf.cpp
*
* Created on: Aug 11, 2009
* Author: Jonathan Brandmeyer
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*
*/
#include "ins_qkf.hpp"
#include "assertions.hpp"
using namespace Eigen;
basic_ins_qkf::basic_ins_qkf(
const Vector3d& estimate,
double pos_error, double bias_error, double v_error,
const Vector3d& gyro_white_noise,
const Vector3d& gyro_stability_noise,
const Vector3d& accel_white_noise,
Quaterniond initial_orientation, // this is new
const Vector3d& vel_estimate)
: gyro_stability_noise(gyro_stability_noise)
, gyro_white_noise(gyro_white_noise)
, accel_white_noise(accel_white_noise)
{
avg_state.position = estimate;
avg_state.gyro_bias = Vector3d::Zero();
//avg_state.orientation = Quaterniond::Identity();
avg_state.orientation = initial_orientation;
avg_state.velocity = vel_estimate;
cov << Matrix3d::Identity()*bias_error*bias_error, Matrix<double, 3, 9>::Zero(),
Matrix3d::Zero(), Matrix3d::Identity()*M_PI*M_PI*0.5, Matrix<double, 3, 6>::Zero(),
Matrix<double, 3, 6>::Zero(), Matrix3d::Identity()*pos_error*pos_error, Matrix3d::Zero(),
Matrix<double, 3, 9>::Zero(), Matrix3d::Identity()*v_error*v_error;
assert(is_real());
}
void
basic_ins_qkf::counter_rotate_cov(const Quaterniond&)
{
// Rotate the principle axes of the angular error covariance by the
// mean update.
// TODO: This is only required in the case that the system covariance is
// right-multiplied by the mean. The current design left-multiplies the
// covariance by the mean.
return;
// TODO: There should be an expression that makes both this matrix's
// construction and multiplication much more efficient.
// Matrix<double, 12, 12> counter_rot = Matrix<double, 12, 12>::Identity();
// counter_rot.block<3, 3>(3, 3) = update.cast<double>().conjugate().toRotationMatrix();
// cov = (counter_rot * cov.cast<double>() * counter_rot.transpose()).cast<float>();
}
#if 0
basic_ins_qkf::state
basic_ins_qkf::average_sigma_points(const std::vector<state, aligned_allocator<state> >& points)
{
state ret;
Vector3d sum;
#define avg_vector_field(field) \
sum = Vector3d::Zero(); \
for (auto i = points.begin(), end = points.end(); i != end; ++i) { \
sum += i->field; \
} \
ret.field = sum / points.size()
avg_vector_field(gyro_bias);
avg_vector_field(velocity);
Vector3d p_sum = Vector3d::Zero();
for (auto i = points.begin(), end = points.end(); i != end; ++i) {
p_sum += i->position;
}
ret.position = p_sum / (double)points.size();
std::vector<Quaterniond> quat_points;
quat_points.reserve(points.size());
for (auto i = points.begin(), end = points.end(); i != end; ++i) {
quat_points.push_back(i->orientation);
}
ret.orientation = quaternion_avg_johnson(quat_points).normalized();
return ret;
}
#endif
bool
basic_ins_qkf::is_real(void) const
{
return !(hasNaN(cov) || hasInf(cov)) && avg_state.is_real();
}
Quaterniond
basic_ins_qkf::state::apply_kalman_vec_update(const Matrix<double, 12, 1> update)
{
// std::cout << "***update available***\n"
// << "\tstate: "; print(std::cout);
// std::cout << "\n\tupdate: " << update.transpose() << "\n";
gyro_bias += update.segment<3>(0);
Quaterniond posterior_update = exp<double>(update.segment<3>(3));
orientation = (orientation * posterior_update).normalized();
position += update.segment<3>(6);
velocity += update.segment<3>(9);
assert(is_real());
return posterior_update;
}
#if 0
Quaterniond
basic_ins_qkf::state::apply_left_kalman_vec_update(const Matrix<double, 12, 1> update)
{
// std::cout << "***update available***\n"
// << "\tstate: "; print(std::cout);
// std::cout << "\n\tupdate: " << update.transpose() << "\n";
gyro_bias += update.segment<3>(0);
Quaterniond posterior_update = exp<double>(update.segment<3>(3));
orientation = (posterior_update * orientation).normalized();
position += update.segment<3>(6);
velocity += update.segment<3>(9);
assert(is_real());
return posterior_update;
}
#endif
bool
basic_ins_qkf::state::has_nan(void)const
{
return hasNaN(gyro_bias) || hasNaN(orientation.coeffs())
|| hasNaN(position) || hasNaN(velocity);
}
bool
basic_ins_qkf::state::is_real(void) const
{
return !(hasNaN(gyro_bias) || hasNaN(orientation.coeffs())
|| hasNaN(position) || hasNaN(velocity))
&& !(hasInf(gyro_bias) || hasInf(orientation.coeffs())
|| hasInf(position) || hasInf(velocity));
}
/*
* basic_ins_qkf.cpp
*
* Created on: Aug 11, 2009
* Author: Jonathan Brandmeyer
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*
*/
#include "ins_qkf.hpp"
#include "assertions.hpp"
using namespace Eigen;
basic_ins_qkf::basic_ins_qkf(
const Vector3d& estimate,
double pos_error, double bias_error, double v_error,
const Vector3d& gyro_white_noise,
const Vector3d& gyro_stability_noise,
const Vector3d& accel_white_noise,
Quaterniond initial_orientation, // this is new
const Vector3d& vel_estimate)
: gyro_stability_noise(gyro_stability_noise)
, gyro_white_noise(gyro_white_noise)
, accel_white_noise(accel_white_noise)
{
avg_state.position = estimate;
avg_state.gyro_bias = Vector3d::Zero();
//avg_state.orientation = Quaterniond::Identity();
avg_state.orientation = initial_orientation;
avg_state.velocity = vel_estimate;
cov << Matrix3d::Identity()*bias_error*bias_error, Matrix<double, 3, 9>::Zero(),
Matrix3d::Zero(), Matrix3d::Identity()*M_PI*M_PI*0.5, Matrix<double, 3, 6>::Zero(),
Matrix<double, 3, 6>::Zero(), Matrix3d::Identity()*pos_error*pos_error, Matrix3d::Zero(),
Matrix<double, 3, 9>::Zero(), Matrix3d::Identity()*v_error*v_error;
assert(is_real());
}
void
basic_ins_qkf::counter_rotate_cov(const Quaterniond&)
{
// Rotate the principle axes of the angular error covariance by the
// mean update.
// TODO: This is only required in the case that the system covariance is
// right-multiplied by the mean. The current design left-multiplies the
// covariance by the mean.
return;
// TODO: There should be an expression that makes both this matrix's
// construction and multiplication much more efficient.
// Matrix<double, 12, 12> counter_rot = Matrix<double, 12, 12>::Identity();
// counter_rot.block<3, 3>(3, 3) = update.cast<double>().conjugate().toRotationMatrix();
// cov = (counter_rot * cov.cast<double>() * counter_rot.transpose()).cast<float>();
}
#if 0
basic_ins_qkf::state
basic_ins_qkf::average_sigma_points(const std::vector<state, aligned_allocator<state> >& points)
{
state ret;
Vector3d sum;
#define avg_vector_field(field) \
sum = Vector3d::Zero(); \
for (auto i = points.begin(), end = points.end(); i != end; ++i) { \
sum += i->field; \
} \
ret.field = sum / points.size()
avg_vector_field(gyro_bias);
avg_vector_field(velocity);
Vector3d p_sum = Vector3d::Zero();
for (auto i = points.begin(), end = points.end(); i != end; ++i) {
p_sum += i->position;
}
ret.position = p_sum / (double)points.size();
std::vector<Quaterniond> quat_points;
quat_points.reserve(points.size());
for (auto i = points.begin(), end = points.end(); i != end; ++i) {
quat_points.push_back(i->orientation);
}
ret.orientation = quaternion_avg_johnson(quat_points).normalized();
return ret;
}
#endif
bool
basic_ins_qkf::is_real(void) const
{
return !(hasNaN(cov) || hasInf(cov)) && avg_state.is_real();
}
Quaterniond
basic_ins_qkf::state::apply_kalman_vec_update(const Matrix<double, 12, 1> update)
{
// std::cout << "***update available***\n"
// << "\tstate: "; print(std::cout);
// std::cout << "\n\tupdate: " << update.transpose() << "\n";
gyro_bias += update.segment<3>(0);
Quaterniond posterior_update = exp<double>(update.segment<3>(3));
orientation = (orientation * posterior_update).normalized();
position += update.segment<3>(6);
velocity += update.segment<3>(9);
assert(is_real());
return posterior_update;
}
#if 0
Quaterniond
basic_ins_qkf::state::apply_left_kalman_vec_update(const Matrix<double, 12, 1> update)
{
// std::cout << "***update available***\n"
// << "\tstate: "; print(std::cout);
// std::cout << "\n\tupdate: " << update.transpose() << "\n";
gyro_bias += update.segment<3>(0);
Quaterniond posterior_update = exp<double>(update.segment<3>(3));
orientation = (posterior_update * orientation).normalized();
position += update.segment<3>(6);
velocity += update.segment<3>(9);
assert(is_real());
return posterior_update;
}
#endif
bool
basic_ins_qkf::state::has_nan(void)const
{
return hasNaN(gyro_bias) || hasNaN(orientation.coeffs())
|| hasNaN(position) || hasNaN(velocity);
}
bool
basic_ins_qkf::state::is_real(void) const
{
return !(hasNaN(gyro_bias) || hasNaN(orientation.coeffs())
|| hasNaN(position) || hasNaN(velocity))
&& !(hasInf(gyro_bias) || hasInf(orientation.coeffs())
|| hasInf(position) || hasInf(velocity));
}
sw/airborne/fms/libeknav/ins_qkf.hpp
+312 -312
View File
File diff suppressed because it is too large Load Diff
sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp
+121 -121
View File
@@ -1,121 +1,121 @@
/*
* ins_qkf_observe_gps_pvt.cpp
*
* Created on: Sep 2, 2009
* Author: Jonathan Brandmeyer
*
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*/
#include "ins_qkf.hpp"
#include "assertions.hpp"
#include <Eigen/LU>
#ifdef TIME_OPS
#include "timer.hpp"
#include <iostream>
#endif
using namespace Eigen;
#define RANK_ONE_UPDATES
void
basic_ins_qkf::obs_gps_v_report(const Vector3d& vel, const Vector3d& v_error)
{
Vector3d residual = vel - avg_state.velocity;
//std::cout << "diff_v(" <<residual(0) << ", " << residual(1) << ", " << residual(2) <<")\n";
#ifdef RANK_ONE_UPDATES
Matrix<double, 12, 1> update = Matrix<double, 12, 1>::Zero();
for (int i = 0; i < 3; ++i) {
double innovation_cov_inv = 1.0/(cov(9+i, 9+i) + v_error[i]);
Matrix<double, 12, 1> gain = cov.block<12, 1>(0, 9+i) * innovation_cov_inv;
update += gain * (residual[i] - update[9+i]);
cov -= gain * cov.block<1, 12>(9+i, 0);
}
#else
Matrix<double, 3, 3> innovation_cov = cov.block<3, 3>(9, 9);
innovation_cov += v_error.asDiagonal();
Matrix<double, 3, 12> kalman_gain_t;
innovation_cov.qr().solve(cov.block<3, 12>(9, 0), &kalman_gain_t);
cov.part<Eigen::SelfAdjoint>() -= cov.block<12, 3>(0, 9) * kalman_gain_t; // .transpose() * cov.block<3, 12>(9, 0);
Matrix<double, 12, 1> update = kalman_gain_t.transpose() * residual;
#endif
//std::cout << "update(" << update.segment<6>(0).transpose()*180/M_PI << "\t" << update.segment<6>(6).transpose() << ")\n";
//update.segment<6>(0) = Matrix<double, 6, 1>::Zero(); // only for debugging
//std::cout << "update(" << update.segment<6>(0).transpose()*180/M_PI << "\t" << update.segment<6>(6).transpose() << ")\n";
Quaterniond rotor = avg_state.apply_kalman_vec_update(update);
counter_rotate_cov(rotor);
assert(is_real());
}
void
basic_ins_qkf::obs_gps_pv_report(
const Vector3d& pos, const Vector3d& vel,
const Vector3d& p_error, const Vector3d v_error)
{
#ifdef TIME_OPS
timer clock;
clock.start();
#endif
#if 1
//std::cout<<"position"<<std::endl;
obs_gps_p_report(pos, p_error);
//std::cout<<"velocity"<<std::endl;
obs_gps_v_report(vel, v_error);
#else
// The observation model is strictly linear here, so use the linear
// form of the kalman gain and update
Matrix<double, 6, 12> obs_matrix;
obs_matrix << Matrix<double, 6, 6>::Zero(), Matrix<double, 6, 6>::Identity();
Matrix<double, 6, 1> residual;
residual.segment<3>(0) = (pos - avg_state.position);
residual.segment<3>(3) = vel - avg_state.velocity;
Matrix<double, 6, 6> innovation_cov = cov.corner<6, 6>(Eigen::BottomRight);
//innovation_cov = obs_matrix * cov * obs_matrix.transpose();
innovation_cov.corner<3, 3>(Eigen::TopLeft) += p_error.asDiagonal();
innovation_cov.corner<3, 3>(Eigen::BottomRight) += v_error.asDiagonal();
#if 1
// Perform matrix inverse by LU decomposition instead of cofactor expansion.
// K = P*transpose(H)*inverse(S)
// K = P*transpose(transpose(transpose(H)*inverse(S)))
// K = P*transpose(transpose(inverse(S))*H)
// K = P*transpose(inverse(transpose(S))*H)
// S == transpose(S)
// K = P*transpose(inverse(S)*H)
// obs_matrx <- inverse(S)*H
Matrix<double, 6, 12> inv_s_h;
innovation_cov.qr().solve(obs_matrix, &inv_s_h);
Matrix<double, 12, 6> kalman_gain = cov * inv_s_h.transpose();
#else
Matrix<double, 12, 6> kalman_gain = cov.block<12, 6>(0, 6) * innovation_cov.inverse();
#endif
Quaterniond rotor = avg_state.apply_kalman_vec_update(kalman_gain * residual);
cov.part<Eigen::SelfAdjoint>() -= kalman_gain * obs_matrix * cov;
counter_rotate_cov(rotor);
assert(is_real());
#endif
#ifdef TIME_OPS
double time = clock.stop() * 1e6;
std::cout << "obs_gps_pvt time: " << time << "\n";
#endif
}
/*
* ins_qkf_observe_gps_pvt.cpp
*
* Created on: Sep 2, 2009
* Author: Jonathan Brandmeyer
*
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*/
#include "ins_qkf.hpp"
#include "assertions.hpp"
#include <Eigen/LU>
#ifdef TIME_OPS
#include "timer.hpp"
#include <iostream>
#endif
using namespace Eigen;
#define RANK_ONE_UPDATES
void
basic_ins_qkf::obs_gps_v_report(const Vector3d& vel, const Vector3d& v_error)
{
Vector3d residual = vel - avg_state.velocity;
//std::cout << "diff_v(" <<residual(0) << ", " << residual(1) << ", " << residual(2) <<")\n";
#ifdef RANK_ONE_UPDATES
Matrix<double, 12, 1> update = Matrix<double, 12, 1>::Zero();
for (int i = 0; i < 3; ++i) {
double innovation_cov_inv = 1.0/(cov(9+i, 9+i) + v_error[i]);
Matrix<double, 12, 1> gain = cov.block<12, 1>(0, 9+i) * innovation_cov_inv;
update += gain * (residual[i] - update[9+i]);
cov -= gain * cov.block<1, 12>(9+i, 0);
}
#else
Matrix<double, 3, 3> innovation_cov = cov.block<3, 3>(9, 9);
innovation_cov += v_error.asDiagonal();
Matrix<double, 3, 12> kalman_gain_t;
innovation_cov.qr().solve(cov.block<3, 12>(9, 0), &kalman_gain_t);
cov.part<Eigen::SelfAdjoint>() -= cov.block<12, 3>(0, 9) * kalman_gain_t; // .transpose() * cov.block<3, 12>(9, 0);
Matrix<double, 12, 1> update = kalman_gain_t.transpose() * residual;
#endif
//std::cout << "update(" << update.segment<6>(0).transpose()*180/M_PI << "\t" << update.segment<6>(6).transpose() << ")\n";
//update.segment<6>(0) = Matrix<double, 6, 1>::Zero(); // only for debugging
//std::cout << "update(" << update.segment<6>(0).transpose()*180/M_PI << "\t" << update.segment<6>(6).transpose() << ")\n";
Quaterniond rotor = avg_state.apply_kalman_vec_update(update);
counter_rotate_cov(rotor);
assert(is_real());
}
void
basic_ins_qkf::obs_gps_pv_report(
const Vector3d& pos, const Vector3d& vel,
const Vector3d& p_error, const Vector3d v_error)
{
#ifdef TIME_OPS
timer clock;
clock.start();
#endif
#if 1
//std::cout<<"position"<<std::endl;
obs_gps_p_report(pos, p_error);
//std::cout<<"velocity"<<std::endl;
obs_gps_v_report(vel, v_error);
#else
// The observation model is strictly linear here, so use the linear
// form of the kalman gain and update
Matrix<double, 6, 12> obs_matrix;
obs_matrix << Matrix<double, 6, 6>::Zero(), Matrix<double, 6, 6>::Identity();
Matrix<double, 6, 1> residual;
residual.segment<3>(0) = (pos - avg_state.position);
residual.segment<3>(3) = vel - avg_state.velocity;
Matrix<double, 6, 6> innovation_cov = cov.corner<6, 6>(Eigen::BottomRight);
//innovation_cov = obs_matrix * cov * obs_matrix.transpose();
innovation_cov.corner<3, 3>(Eigen::TopLeft) += p_error.asDiagonal();
innovation_cov.corner<3, 3>(Eigen::BottomRight) += v_error.asDiagonal();
#if 1
// Perform matrix inverse by LU decomposition instead of cofactor expansion.
// K = P*transpose(H)*inverse(S)
// K = P*transpose(transpose(transpose(H)*inverse(S)))
// K = P*transpose(transpose(inverse(S))*H)
// K = P*transpose(inverse(transpose(S))*H)
// S == transpose(S)
// K = P*transpose(inverse(S)*H)
// obs_matrx <- inverse(S)*H
Matrix<double, 6, 12> inv_s_h;
innovation_cov.qr().solve(obs_matrix, &inv_s_h);
Matrix<double, 12, 6> kalman_gain = cov * inv_s_h.transpose();
#else
Matrix<double, 12, 6> kalman_gain = cov.block<12, 6>(0, 6) * innovation_cov.inverse();
#endif
Quaterniond rotor = avg_state.apply_kalman_vec_update(kalman_gain * residual);
cov.part<Eigen::SelfAdjoint>() -= kalman_gain * obs_matrix * cov;
counter_rotate_cov(rotor);
assert(is_real());
#endif
#ifdef TIME_OPS
double time = clock.stop() * 1e6;
std::cout << "obs_gps_pvt time: " << time << "\n";
#endif
}
sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp
+193 -193
View File
@@ -1,193 +1,193 @@
/*
* ins_qkf_observe_vector.cpp
*
* Created on: Sep 2, 2009
* Author: Jonathan Brandmeyer
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*/
#include "ins_qkf.hpp"
#include "assertions.hpp"
#ifdef TIME_OPS
#include "timer.hpp"
#include <iostream>
#endif
using namespace Eigen;
#define RANK_ONE_UPDATES
#include <Eigen/LU>
void
basic_ins_qkf::obs_gyro_bias(const Vector3d& bias, const Vector3d& bias_error)
{
Matrix<double, 12, 3> kalman_gain = cov.block<12, 3>(0, 0)
* (cov.block<3, 3>(0, 0) + bias_error.asDiagonal()).inverse();
cov -= kalman_gain * cov.block<3, 12>(0, 0);
Vector3d innovation = bias - avg_state.gyro_bias;
// Apply the Kalman gain to obtain the posterior state and error estimates.
avg_state.apply_kalman_vec_update(kalman_gain * innovation);
}
void
basic_ins_qkf::obs_vector(const Vector3d& ref, const Vector3d& obs, double error)
{
#ifdef TIME_OPS
timer clock;
clock.start();
#endif
#define DEBUG_VECTOR_OBS 0
// Optimization opportunity: re-use sigma points from the state prediction
#if 0
std::vector<vector_obs_state, aligned_allocator<vector_obs_state> > points;
Matrix<double, 15, 30> state_errors;
decompose_sigma_points(points, state_errors, error);
std::vector<Quaterniond> projected_points;
projected_points.reserve(points.size());
for (auto i = points.begin(), end = points.end(); i != end; ++i) {
projected_points.push_back(observe_vector(*i, ref));
}
Vector3d expected_obs = avg_state.orientation * ref;
Quaterniond expected_observation_prediction = Quaterniond().setFromTwoVectors(ref, expected_obs);
// Compute the observation error matrix and its self-covariance
Quaterniond mean_observation_pred = quaternion_avg_johnson(projected_points);
// assert(obs_projection.transpose().isUnitary());
#if DEBUG_VECTOR_OBS
std::cout << "\n\nref: " << ref.transpose();
std::cout << "\nobs: " << obs.transpose();
std::cout << "\nexpected observation: " << expected_observation_prediction.coeffs().transpose();
std::cout << "\nUKF observation: " << mean_observation_pred.coeffs().transpose();
std::cout << "\nexpected innovation: " << log<double>(Quaterniond().setFromTwoVectors(
expected_obs, obs)).transpose();
std::cout << "\ntangent space innovation: " << log<double>(avg_state.orientation.conjugate() *
Quaterniond().setFromTwoVectors(expected_obs, obs) * avg_state.orientation) << "\n";
#endif
Matrix<double, 3, Dynamic> obs_errors(3, projected_points.size());
for (unsigned i = 0; i != projected_points.size(); ++i) {
if (projected_points[i].coeffs().dot(mean_observation_pred.coeffs()) < 0) {
// Choose the point on the same hemisphere as the mean. otherwise, the error vectors
// get screwed up.
projected_points[i].coeffs() *= -1;
}
obs_errors.col(i) = log<double>(mean_observation_pred.conjugate() * projected_points[i]);
}
// Construct an observation matrix composed of the two unit vectors
// orthogonal to the direction that pivots about the expected observation
// Vector3d obs = obs.normalized();
Matrix<double, 2, 3> obs_projection;
#if 1
SelfAdjointEigenSolver<Matrix<double, 3, 3> > obs_cov((obs_errors * obs_errors.transpose())*0.5);
obs_projection = obs_cov.eigenvectors().block<3, 2>(0, 1).transpose();
#else
typedef Vector3d vector_t;
obs_projection.row(0) = expected_obs.cross((expected_obs.dot(vector_t::UnitX()) < 0.707)
? vector_t::UnitX() : vector_t::UnitY()).normalized();
obs_projection.row(1) = expected_obs.cross(obs_projection.row(0));
#endif
assert(!hasNaN(obs_projection));
#if DEBUG_VECTOR_OBS
std::cout << "predicted obs: " << expected_obs.transpose() << "\n";
std::cout << "actual obs: " << obs.transpose() << "\n";
std::cout << "reference obs: " << ref.transpose() << "\n";
std::cout << "obs cov eigenvalues: " << obs_cov.eigenvalues().transpose() << "\n";
std::cout << "obs cov eigenvectors:\n" << obs_cov.eigenvectors() << "\n";
std::cout << "measurement subspace:\n" << obs_projection.transpose() << "\n";
#endif
Matrix<double, 2, Dynamic> projected_obs_errors = obs_projection*obs_errors;
Matrix<double, 2, 2> obs_pred_cov = (projected_obs_errors * projected_obs_errors.transpose())*0.5;
#if DEBUG_VECTOR_OBS
std::cout << "obs errors: " << obs_errors << "\n";
std::cout << "S: " << obs_pred_cov << "\n";
std::cout << "expected S: " << cov.block<3, 3>(3, 3) << "\n";
#endif
Matrix<double, 12, 2> state_meas_cross_cov =
(state_errors.block<12, 30>(0,0) * projected_obs_errors.transpose())*0.5;
Matrix<double, 12, 2> kalman_gain = state_meas_cross_cov * obs_pred_cov.inverse();
// Innovation rotation in the tangent space at the mean
Vector2d innovation = obs_projection*log<double>(
Quaterniond().setFromTwoVectors((mean_observation_pred * ref), obs));
#if DEBUG_VECTOR_OBS
std::cout << "inverse(S): " << obs_pred_cov.inverse() << "\n";
std::cout << "used innovation: " << log<double>(Quaterniond().setFromTwoVectors(
mean_observation_pred * ref, obs)).transpose() << "\n";
std::cout << "projected innovation: " << innovation.transpose() << "\n";
std::cout << "deprojected innovation: " << (obs_projection.transpose() * innovation).transpose() << "\n";
#endif
// actually, cov -= K*S*K^T, but this saves a multiplication.
cov -= (kalman_gain * obs_pred_cov * kalman_gain.transpose());
#else
// BIG optimization opportunity: Use a pseudo-linear measurement model.
Vector3d obs_ref = avg_state.orientation.conjugate()*obs;
Vector3d v_residual = log<double>(Quaterniond().setFromTwoVectors(ref, obs_ref));
Matrix<double, 3, 2> h_trans;
h_trans.col(0) = ref.cross(
(abs(ref.dot(obs_ref)) < 0.9994) ? obs_ref :
(abs(ref.dot(Vector3d::UnitX())) < 0.707)
? Vector3d::UnitX() : Vector3d::UnitY()).normalized();
h_trans.col(1) = -ref.cross(h_trans.col(0));
assert(!hasNaN(h_trans));
assert(h_trans.isUnitary());
Vector2d innovation = h_trans.transpose() * v_residual;
#ifdef RANK_ONE_UPDATES
// Running a rank-one update here is a strict win.
Matrix<double, 12, 1> update = Matrix<double, 12, 1>::Zero();
for (int i = 0; i < 2; ++i) {
double obs_error = error;
double obs_cov = (h_trans.col(i).transpose() * cov.block<3, 3>(3, 3) * h_trans.col(i))[0];
Matrix<double, 12, 1> gain = cov.block<12, 3>(0, 3) * h_trans.col(i) / (obs_error + obs_cov);
update += gain * h_trans.col(i).transpose() * v_residual;
cov -= gain * h_trans.col(i).transpose() * cov.block<3, 12>(3, 0);
}
#else
Matrix<double, 12, 2> kalman_gain = cov.block<12, 3>(0, 3) * h_trans
* (h_trans.transpose() * cov.block<3, 3>(3, 3) * h_trans
+ (Vector2d() << error, error).finished().asDiagonal()).inverse();
cov -= kalman_gain * h_trans.transpose() * cov.block<3, 12>(3, 0);
#endif
#endif
// Apply the Kalman gain to obtain the posterior state and error estimates.
#ifndef RANK_ONE_UPDATES
Matrix<double, 12, 1> update = (kalman_gain * innovation);
#endif
#if DEBUG_VECTOR_OBS
// std::cout << "projected update: " << (obs_projection * update.segment<3>(3)).transpose() << "\n";
std::cout << "deprojected update: " << update.segment<3>(3).transpose() << "\n";
#endif
Quaterniond posterior_update = avg_state.apply_kalman_vec_update(update);
counter_rotate_cov(posterior_update);
assert(is_real());
#ifdef TIME_OPS
double time = clock.stop() * 1e6;
std::cout << "observe_vector(): " << time << "\n";
#endif
}
/*
* ins_qkf_observe_vector.cpp
*
* Created on: Sep 2, 2009
* Author: Jonathan Brandmeyer
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*/
#include "ins_qkf.hpp"
#include "assertions.hpp"
#ifdef TIME_OPS
#include "timer.hpp"
#include <iostream>
#endif
using namespace Eigen;
#define RANK_ONE_UPDATES
#include <Eigen/LU>
void
basic_ins_qkf::obs_gyro_bias(const Vector3d& bias, const Vector3d& bias_error)
{
Matrix<double, 12, 3> kalman_gain = cov.block<12, 3>(0, 0)
* (cov.block<3, 3>(0, 0) + bias_error.asDiagonal()).inverse();
cov -= kalman_gain * cov.block<3, 12>(0, 0);
Vector3d innovation = bias - avg_state.gyro_bias;
// Apply the Kalman gain to obtain the posterior state and error estimates.
avg_state.apply_kalman_vec_update(kalman_gain * innovation);
}
void
basic_ins_qkf::obs_vector(const Vector3d& ref, const Vector3d& obs, double error)
{
#ifdef TIME_OPS
timer clock;
clock.start();
#endif
#define DEBUG_VECTOR_OBS 0
// Optimization opportunity: re-use sigma points from the state prediction
#if 0
std::vector<vector_obs_state, aligned_allocator<vector_obs_state> > points;
Matrix<double, 15, 30> state_errors;
decompose_sigma_points(points, state_errors, error);
std::vector<Quaterniond> projected_points;
projected_points.reserve(points.size());
for (auto i = points.begin(), end = points.end(); i != end; ++i) {
projected_points.push_back(observe_vector(*i, ref));
}
Vector3d expected_obs = avg_state.orientation * ref;
Quaterniond expected_observation_prediction = Quaterniond().setFromTwoVectors(ref, expected_obs);
// Compute the observation error matrix and its self-covariance
Quaterniond mean_observation_pred = quaternion_avg_johnson(projected_points);
// assert(obs_projection.transpose().isUnitary());
#if DEBUG_VECTOR_OBS
std::cout << "\n\nref: " << ref.transpose();
std::cout << "\nobs: " << obs.transpose();
std::cout << "\nexpected observation: " << expected_observation_prediction.coeffs().transpose();
std::cout << "\nUKF observation: " << mean_observation_pred.coeffs().transpose();
std::cout << "\nexpected innovation: " << log<double>(Quaterniond().setFromTwoVectors(
expected_obs, obs)).transpose();
std::cout << "\ntangent space innovation: " << log<double>(avg_state.orientation.conjugate() *
Quaterniond().setFromTwoVectors(expected_obs, obs) * avg_state.orientation) << "\n";
#endif
Matrix<double, 3, Dynamic> obs_errors(3, projected_points.size());
for (unsigned i = 0; i != projected_points.size(); ++i) {
if (projected_points[i].coeffs().dot(mean_observation_pred.coeffs()) < 0) {
// Choose the point on the same hemisphere as the mean. otherwise, the error vectors
// get screwed up.
projected_points[i].coeffs() *= -1;
}
obs_errors.col(i) = log<double>(mean_observation_pred.conjugate() * projected_points[i]);
}
// Construct an observation matrix composed of the two unit vectors
// orthogonal to the direction that pivots about the expected observation
// Vector3d obs = obs.normalized();
Matrix<double, 2, 3> obs_projection;
#if 1
SelfAdjointEigenSolver<Matrix<double, 3, 3> > obs_cov((obs_errors * obs_errors.transpose())*0.5);
obs_projection = obs_cov.eigenvectors().block<3, 2>(0, 1).transpose();
#else
typedef Vector3d vector_t;
obs_projection.row(0) = expected_obs.cross((expected_obs.dot(vector_t::UnitX()) < 0.707)
? vector_t::UnitX() : vector_t::UnitY()).normalized();
obs_projection.row(1) = expected_obs.cross(obs_projection.row(0));
#endif
assert(!hasNaN(obs_projection));
#if DEBUG_VECTOR_OBS
std::cout << "predicted obs: " << expected_obs.transpose() << "\n";
std::cout << "actual obs: " << obs.transpose() << "\n";
std::cout << "reference obs: " << ref.transpose() << "\n";
std::cout << "obs cov eigenvalues: " << obs_cov.eigenvalues().transpose() << "\n";
std::cout << "obs cov eigenvectors:\n" << obs_cov.eigenvectors() << "\n";
std::cout << "measurement subspace:\n" << obs_projection.transpose() << "\n";
#endif
Matrix<double, 2, Dynamic> projected_obs_errors = obs_projection*obs_errors;
Matrix<double, 2, 2> obs_pred_cov = (projected_obs_errors * projected_obs_errors.transpose())*0.5;
#if DEBUG_VECTOR_OBS
std::cout << "obs errors: " << obs_errors << "\n";
std::cout << "S: " << obs_pred_cov << "\n";
std::cout << "expected S: " << cov.block<3, 3>(3, 3) << "\n";
#endif
Matrix<double, 12, 2> state_meas_cross_cov =
(state_errors.block<12, 30>(0,0) * projected_obs_errors.transpose())*0.5;
Matrix<double, 12, 2> kalman_gain = state_meas_cross_cov * obs_pred_cov.inverse();
// Innovation rotation in the tangent space at the mean
Vector2d innovation = obs_projection*log<double>(
Quaterniond().setFromTwoVectors((mean_observation_pred * ref), obs));
#if DEBUG_VECTOR_OBS
std::cout << "inverse(S): " << obs_pred_cov.inverse() << "\n";
std::cout << "used innovation: " << log<double>(Quaterniond().setFromTwoVectors(
mean_observation_pred * ref, obs)).transpose() << "\n";
std::cout << "projected innovation: " << innovation.transpose() << "\n";
std::cout << "deprojected innovation: " << (obs_projection.transpose() * innovation).transpose() << "\n";
#endif
// actually, cov -= K*S*K^T, but this saves a multiplication.
cov -= (kalman_gain * obs_pred_cov * kalman_gain.transpose());
#else
// BIG optimization opportunity: Use a pseudo-linear measurement model.
Vector3d obs_ref = avg_state.orientation.conjugate()*obs;
Vector3d v_residual = log<double>(Quaterniond().setFromTwoVectors(ref, obs_ref));
Matrix<double, 3, 2> h_trans;
h_trans.col(0) = ref.cross(
(abs(ref.dot(obs_ref)) < 0.9994) ? obs_ref :
(abs(ref.dot(Vector3d::UnitX())) < 0.707)
? Vector3d::UnitX() : Vector3d::UnitY()).normalized();
h_trans.col(1) = -ref.cross(h_trans.col(0));
assert(!hasNaN(h_trans));
assert(h_trans.isUnitary());
Vector2d innovation = h_trans.transpose() * v_residual;
#ifdef RANK_ONE_UPDATES
// Running a rank-one update here is a strict win.
Matrix<double, 12, 1> update = Matrix<double, 12, 1>::Zero();
for (int i = 0; i < 2; ++i) {
double obs_error = error;
double obs_cov = (h_trans.col(i).transpose() * cov.block<3, 3>(3, 3) * h_trans.col(i))[0];
Matrix<double, 12, 1> gain = cov.block<12, 3>(0, 3) * h_trans.col(i) / (obs_error + obs_cov);
update += gain * h_trans.col(i).transpose() * v_residual;
cov -= gain * h_trans.col(i).transpose() * cov.block<3, 12>(3, 0);
}
#else
Matrix<double, 12, 2> kalman_gain = cov.block<12, 3>(0, 3) * h_trans
* (h_trans.transpose() * cov.block<3, 3>(3, 3) * h_trans
+ (Vector2d() << error, error).finished().asDiagonal()).inverse();
cov -= kalman_gain * h_trans.transpose() * cov.block<3, 12>(3, 0);
#endif
#endif
// Apply the Kalman gain to obtain the posterior state and error estimates.
#ifndef RANK_ONE_UPDATES
Matrix<double, 12, 1> update = (kalman_gain * innovation);
#endif
#if DEBUG_VECTOR_OBS
// std::cout << "projected update: " << (obs_projection * update.segment<3>(3)).transpose() << "\n";
std::cout << "deprojected update: " << update.segment<3>(3).transpose() << "\n";
#endif
Quaterniond posterior_update = avg_state.apply_kalman_vec_update(update);
counter_rotate_cov(posterior_update);
assert(is_real());
#ifdef TIME_OPS
double time = clock.stop() * 1e6;
std::cout << "observe_vector(): " << time << "\n";
#endif
}
sw/airborne/fms/libeknav/ins_qkf_predict.cpp
+184 -184
View File
@@ -1,184 +1,184 @@
/*
* ins_qkf_predict.cpp
*
* Created on: Sep 2, 2009
* Author: Jonathan Brandmeyer
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*/
/**
* This is a modified version by Martin Dieblich
* The general changes are a more consequent naming
*
*/
#include "ins_qkf.hpp"
#include "assertions.hpp"
#ifdef TIME_OPS
//#include "timer.hpp"
#include <iostream>
#endif
using namespace Eigen;
namespace {
Matrix<double, 3, 3>
axis_scale(const Vector3d& axis, double scale)
{
return (scale - 1) * axis * axis.transpose() + Matrix3d::Identity();
}
void
linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const Vector3d& accel_meas, double dt)
{
// The two components of rotation that do not spin about the gravity vector
// have an influence on the position and velocity of the vehicle.
// Let r be an error axis of rotation, and z be the gravity vector.
// Increasing r creates increasing error in the direction _|_ to r and z.
// By the small angle theorem, the amount of error is ~ abs(r)*abs(z).
// Increasing r also creates increasing error in the direction || to -z.
// By the small angle theorem, the amount of error is ~ zero.
// Therefore, rotate the error block about the z axis by -90 degrees, and
// zero out the error vector in the z direction.
// accel_cov is the relationship between error vectors in the tangent space
// of the vehicle orientation and the translational reference frame.
Matrix3d rot = _this.avg_state.orientation.conjugate().toRotationMatrix();
Vector3d accel_ecef = rot*accel_meas; // a_e = (q_e2b)^* x a_b = q_b2e x a_b
Vector3d accel_gravity = _this.avg_state.position.normalized()*(-9.81);
Vector3d accel_resid = accel_ecef + accel_gravity; // a = (xdd-g)+g = xdd;
#if 0
printf("==================================================\n");
printf("Quaternion: % 1.4f % 1.4f % 1.4f % 1.4f\n",
_this.avg_state.orientation.w(),
_this.avg_state.orientation.x(),
_this.avg_state.orientation.y(),
_this.avg_state.orientation.z());
printf("Accel_meas: % 1.4f % 1.4f % 1.4f\n", accel_meas(0), accel_meas(1), accel_meas(2));
printf("Accel_ecef: % 1.4f % 1.4f % 1.4f\n", accel_ecef(0), accel_ecef(1), accel_ecef(2));
printf("Accel_grav: % 1.4f % 1.4f % 1.4f\n", accel_gravity(0), accel_gravity(1), accel_gravity(2));
#endif
#if 0
// This form works well with zero static acceleration.
Matrix<double, 3, 3> accel_cov =
Eigen::AngleAxisd(-M_PI*0.5, _this.avg_state.position.normalized())
* axis_scale(_this.avg_state.position.normalized(), 0) * 9.81;
#elif 1
Matrix<double, 3, 3> accel_cov =
Eigen::AngleAxisd(-M_PI*0.5, accel_ecef.normalized())
* axis_scale(accel_ecef.normalized(), 0) * accel_meas.norm();
#else
// The following form ends up being identical to the simpler one
// above
Matrix<double, 3, 3> accel_cov =
Eigen::AngleAxisd(-M_PI*0.5, _this.avg_state.position.normalized())
* axis_scale(_this.avg_state.position.normalized(), 0) * 9.81
+ Eigen::AngleAxisd(-M_PI*0.5, accel_resid.normalized())
* axis_scale(accel_resid.normalized(), 0)*accel_resid.norm();
#endif
// TODO: Optimization opportunity: the accel_cov doesn't change much over
// the life of a mission. Precompute it once and then retain the original.
// Then, only one 3x3 block ever gets updated in the A matrix below.
// The linearized Kalman state projection matrix.
#if 0
Matrix<double, 12, 12> A;
// gyro bias row
A << Matrix<double, 3, 3>::Identity(), Matrix<double, 3, 9>::Zero(),
// Orientation row
_this.avg_state.orientation.conjugate().toRotationMatrix()*-dt,
Matrix<double, 3, 3>::Identity(), Matrix<double, 3, 6>::Zero(),
// Position row
Matrix<double, 3, 3>::Zero(), -accel_cov*0.5*dt*dt,
Matrix<double, 3, 3>::Identity(), Matrix<double, 3, 3>::Identity()*dt,
// Velocity row
Matrix<double, 3, 3>::Zero(), -accel_cov * dt,
Matrix<double, 3, 3>::Zero(), Matrix<double, 3, 3>::Identity();
// 800x realtime, with vectorization
_this.cov.part<Eigen::SelfAdjoint>() = A * _this.cov * A.transpose();
#else
// 1500x realtime, without vectorization, on 2.2 GHz Athlon X2
const Matrix<double, 12, 12> cov = _this.cov;
const Matrix3d dtR = dt * _this.avg_state.orientation.conjugate().toRotationMatrix();
const Matrix3d dtQ = accel_cov * dt;
_this.cov.block<3, 3>(0, 3) -= cov.block<3,3>(0, 0)*dtR.transpose();
_this.cov.block<3, 3>(0, 6) += dt * cov.block<3, 3>(0, 9);
_this.cov.block<3, 3>(0, 9) -= cov.block<3, 3>(0, 3) * dtQ.transpose();
_this.cov.block<3, 3>(3, 3).part<Eigen::SelfAdjoint>() += dtR*cov.block<3, 3>(0, 0)*dtR.transpose()
- dtR*cov.block<3, 3>(0, 3) - cov.block<3, 3>(3, 0)*dtR.transpose();
_this.cov.block<3, 3>(3, 6) += -dtR * (cov.block<3, 3>(0, 6) + dt*cov.block<3, 3>(0, 9))
+ dt*cov.block<3, 3>(3, 9);
_this.cov.block<3, 3>(3, 9) += -dtR*( -cov.block<3, 3>(0, 3)*dtQ.transpose() + cov.block<3, 3>(0, 9))
- cov.block<3, 3>(3, 3)*dtQ.transpose();
_this.cov.block<3, 3>(6, 6).part<Eigen::SelfAdjoint>() += dt*cov.block<3, 3>(6, 9) + dt*dt*cov.block<3, 3>(9, 9)
+ dt*cov.block<3, 3>(9, 6);
_this.cov.block<3, 3>(6, 9) += -cov.block<3, 3>(6, 3)*dtQ.transpose() + dt*cov.block<3, 3>(9, 9)
- dt*cov.block<3, 3>(9, 3)*dtQ.transpose();
_this.cov.block<3, 3>(9, 9).part<Eigen::SelfAdjoint>() += dtQ*cov.block<3, 3>(3, 3)*dtQ.transpose()
- dtQ*cov.block<3, 3>(3, 9) - cov.block<3, 3>(9, 3)*dtQ.transpose();
_this.cov.block<3, 3>(3, 0) = _this.cov.block<3, 3>(0, 3).transpose();
_this.cov.block<3, 3>(6, 0) = _this.cov.block<3, 3>(0, 6).transpose();
_this.cov.block<3, 3>(6, 3) = _this.cov.block<3, 3>(3, 6).transpose();
_this.cov.block<3, 3>(9, 0) = _this.cov.block<3, 3>(0, 9).transpose();
_this.cov.block<3, 3>(9, 3) = _this.cov.block<3, 3>(3, 9).transpose();
_this.cov.block<3, 3>(9, 6) = _this.cov.block<3, 3>(6, 9).transpose();
#endif
_this.cov.block<3, 3>(0, 0) += _this.gyro_stability_noise.asDiagonal() * dt;
_this.cov.block<3, 3>(3, 3) += _this.gyro_white_noise.asDiagonal() * dt;
_this.cov.block<3, 3>(6, 6) += _this.accel_white_noise.asDiagonal() * 0.5*dt*dt;
_this.cov.block<3, 3>(9, 9) += _this.accel_white_noise.asDiagonal() * dt;
Quaterniond orientation = exp<double>((gyro_meas - _this.avg_state.gyro_bias) * dt)
* _this.avg_state.orientation;
//Vector3d accel = accel_ecef - _this.avg_state.position.normalized() * 9.81;
//std::cout << " ACCEL______(" << accel.transpose() << ")\n";
Vector3d position = _this.avg_state.position + _this.avg_state.velocity * dt + 0.5*accel_resid*dt*dt;
Vector3d velocity = _this.avg_state.velocity + accel_resid*dt;
_this.avg_state.position = position;
_this.avg_state.velocity = velocity;
_this.avg_state.orientation = orientation;
}
} // !namespace (anon)
void
basic_ins_qkf::predict(const Vector3d& gyro_meas, const Vector3d& accel_meas, double dt)
{
#ifdef TIME_OPS
timer clock;
clock.start();
#endif
// Always use linearized prediction
linear_predict(*this, gyro_meas, accel_meas, dt);
#ifdef TIME_OPS
double time = clock.stop()*1e6;
std::cout << "unscented predict time: " << time << "\n";
#endif
}
/*
* ins_qkf_predict.cpp
*
* Created on: Sep 2, 2009
* Author: Jonathan Brandmeyer
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*/
/**
* This is a modified version by Martin Dieblich
* The general changes are a more consequent naming
*
*/
#include "ins_qkf.hpp"
#include "assertions.hpp"
#ifdef TIME_OPS
//#include "timer.hpp"
#include <iostream>
#endif
using namespace Eigen;
namespace {
Matrix<double, 3, 3>
axis_scale(const Vector3d& axis, double scale)
{
return (scale - 1) * axis * axis.transpose() + Matrix3d::Identity();
}
void
linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const Vector3d& accel_meas, double dt)
{
// The two components of rotation that do not spin about the gravity vector
// have an influence on the position and velocity of the vehicle.
// Let r be an error axis of rotation, and z be the gravity vector.
// Increasing r creates increasing error in the direction _|_ to r and z.
// By the small angle theorem, the amount of error is ~ abs(r)*abs(z).
// Increasing r also creates increasing error in the direction || to -z.
// By the small angle theorem, the amount of error is ~ zero.
// Therefore, rotate the error block about the z axis by -90 degrees, and
// zero out the error vector in the z direction.
// accel_cov is the relationship between error vectors in the tangent space
// of the vehicle orientation and the translational reference frame.
Matrix3d rot = _this.avg_state.orientation.conjugate().toRotationMatrix();
Vector3d accel_ecef = rot*accel_meas; // a_e = (q_e2b)^* x a_b = q_b2e x a_b
Vector3d accel_gravity = _this.avg_state.position.normalized()*(-9.81);
Vector3d accel_resid = accel_ecef + accel_gravity; // a = (xdd-g)+g = xdd;
#if 0
printf("==================================================\n");
printf("Quaternion: % 1.4f % 1.4f % 1.4f % 1.4f\n",
_this.avg_state.orientation.w(),
_this.avg_state.orientation.x(),
_this.avg_state.orientation.y(),
_this.avg_state.orientation.z());
printf("Accel_meas: % 1.4f % 1.4f % 1.4f\n", accel_meas(0), accel_meas(1), accel_meas(2));
printf("Accel_ecef: % 1.4f % 1.4f % 1.4f\n", accel_ecef(0), accel_ecef(1), accel_ecef(2));
printf("Accel_grav: % 1.4f % 1.4f % 1.4f\n", accel_gravity(0), accel_gravity(1), accel_gravity(2));
#endif
#if 0
// This form works well with zero static acceleration.
Matrix<double, 3, 3> accel_cov =
Eigen::AngleAxisd(-M_PI*0.5, _this.avg_state.position.normalized())
* axis_scale(_this.avg_state.position.normalized(), 0) * 9.81;
#elif 1
Matrix<double, 3, 3> accel_cov =
Eigen::AngleAxisd(-M_PI*0.5, accel_ecef.normalized())
* axis_scale(accel_ecef.normalized(), 0) * accel_meas.norm();
#else
// The following form ends up being identical to the simpler one
// above
Matrix<double, 3, 3> accel_cov =
Eigen::AngleAxisd(-M_PI*0.5, _this.avg_state.position.normalized())
* axis_scale(_this.avg_state.position.normalized(), 0) * 9.81
+ Eigen::AngleAxisd(-M_PI*0.5, accel_resid.normalized())
* axis_scale(accel_resid.normalized(), 0)*accel_resid.norm();
#endif
// TODO: Optimization opportunity: the accel_cov doesn't change much over
// the life of a mission. Precompute it once and then retain the original.
// Then, only one 3x3 block ever gets updated in the A matrix below.
// The linearized Kalman state projection matrix.
#if 0
Matrix<double, 12, 12> A;
// gyro bias row
A << Matrix<double, 3, 3>::Identity(), Matrix<double, 3, 9>::Zero(),
// Orientation row
_this.avg_state.orientation.conjugate().toRotationMatrix()*-dt,
Matrix<double, 3, 3>::Identity(), Matrix<double, 3, 6>::Zero(),
// Position row
Matrix<double, 3, 3>::Zero(), -accel_cov*0.5*dt*dt,
Matrix<double, 3, 3>::Identity(), Matrix<double, 3, 3>::Identity()*dt,
// Velocity row
Matrix<double, 3, 3>::Zero(), -accel_cov * dt,
Matrix<double, 3, 3>::Zero(), Matrix<double, 3, 3>::Identity();
// 800x realtime, with vectorization
_this.cov.part<Eigen::SelfAdjoint>() = A * _this.cov * A.transpose();
#else
// 1500x realtime, without vectorization, on 2.2 GHz Athlon X2
const Matrix<double, 12, 12> cov = _this.cov;
const Matrix3d dtR = dt * _this.avg_state.orientation.conjugate().toRotationMatrix();
const Matrix3d dtQ = accel_cov * dt;
_this.cov.block<3, 3>(0, 3) -= cov.block<3,3>(0, 0)*dtR.transpose();
_this.cov.block<3, 3>(0, 6) += dt * cov.block<3, 3>(0, 9);
_this.cov.block<3, 3>(0, 9) -= cov.block<3, 3>(0, 3) * dtQ.transpose();
_this.cov.block<3, 3>(3, 3).part<Eigen::SelfAdjoint>() += dtR*cov.block<3, 3>(0, 0)*dtR.transpose()
- dtR*cov.block<3, 3>(0, 3) - cov.block<3, 3>(3, 0)*dtR.transpose();
_this.cov.block<3, 3>(3, 6) += -dtR * (cov.block<3, 3>(0, 6) + dt*cov.block<3, 3>(0, 9))
+ dt*cov.block<3, 3>(3, 9);
_this.cov.block<3, 3>(3, 9) += -dtR*( -cov.block<3, 3>(0, 3)*dtQ.transpose() + cov.block<3, 3>(0, 9))
- cov.block<3, 3>(3, 3)*dtQ.transpose();
_this.cov.block<3, 3>(6, 6).part<Eigen::SelfAdjoint>() += dt*cov.block<3, 3>(6, 9) + dt*dt*cov.block<3, 3>(9, 9)
+ dt*cov.block<3, 3>(9, 6);
_this.cov.block<3, 3>(6, 9) += -cov.block<3, 3>(6, 3)*dtQ.transpose() + dt*cov.block<3, 3>(9, 9)
- dt*cov.block<3, 3>(9, 3)*dtQ.transpose();
_this.cov.block<3, 3>(9, 9).part<Eigen::SelfAdjoint>() += dtQ*cov.block<3, 3>(3, 3)*dtQ.transpose()
- dtQ*cov.block<3, 3>(3, 9) - cov.block<3, 3>(9, 3)*dtQ.transpose();
_this.cov.block<3, 3>(3, 0) = _this.cov.block<3, 3>(0, 3).transpose();
_this.cov.block<3, 3>(6, 0) = _this.cov.block<3, 3>(0, 6).transpose();
_this.cov.block<3, 3>(6, 3) = _this.cov.block<3, 3>(3, 6).transpose();
_this.cov.block<3, 3>(9, 0) = _this.cov.block<3, 3>(0, 9).transpose();
_this.cov.block<3, 3>(9, 3) = _this.cov.block<3, 3>(3, 9).transpose();
_this.cov.block<3, 3>(9, 6) = _this.cov.block<3, 3>(6, 9).transpose();
#endif
_this.cov.block<3, 3>(0, 0) += _this.gyro_stability_noise.asDiagonal() * dt;
_this.cov.block<3, 3>(3, 3) += _this.gyro_white_noise.asDiagonal() * dt;
_this.cov.block<3, 3>(6, 6) += _this.accel_white_noise.asDiagonal() * 0.5*dt*dt;
_this.cov.block<3, 3>(9, 9) += _this.accel_white_noise.asDiagonal() * dt;
Quaterniond orientation = exp<double>((gyro_meas - _this.avg_state.gyro_bias) * dt)
* _this.avg_state.orientation;
//Vector3d accel = accel_ecef - _this.avg_state.position.normalized() * 9.81;
//std::cout << " ACCEL______(" << accel.transpose() << ")\n";
Vector3d position = _this.avg_state.position + _this.avg_state.velocity * dt + 0.5*accel_resid*dt*dt;
Vector3d velocity = _this.avg_state.velocity + accel_resid*dt;
_this.avg_state.position = position;
_this.avg_state.velocity = velocity;
_this.avg_state.orientation = orientation;
}
} // !namespace (anon)
void
basic_ins_qkf::predict(const Vector3d& gyro_meas, const Vector3d& accel_meas, double dt)
{
#ifdef TIME_OPS
timer clock;
clock.start();
#endif
// Always use linearized prediction
linear_predict(*this, gyro_meas, accel_meas, dt);
#ifdef TIME_OPS
double time = clock.stop()*1e6;
std::cout << "unscented predict time: " << time << "\n";
#endif
}
sw/airborne/fms/libeknav/quaternions.hpp
+143 -143
View File
@@ -1,143 +1,143 @@
#ifndef AHRS_QUATERNIONS_HPP
#define AHRS_QUATERNIONS_HPP
/*
* quaternions.cpp
*
* Author: Jonathan Brandmeyer
*
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*/
#include <Eigen/Core>
#include <Eigen/Geometry>
using Eigen::Quaternion;
template<typename FloatT>
Quaternion<FloatT> operator-(const Quaternion<FloatT>& q)
{
return Quaternion<FloatT>(-q.w(), -q.x(), -q.y(), -q.z());
}
/**
* Convert a rotation from modified Rodrigues parameters to a quaternion.
* @param v The multiplication of the rotation angle by the tangent of the angle/4
* @return The quaternion corresponding to that rotation.
*/
template<typename FloatT>
Quaternion<FloatT> exp_r(const Eigen::Matrix<FloatT, 3, 1>& v)
{
// a2 = tan^2(theta/4)
FloatT a2 = v.squaredNorm();
Quaternion<FloatT> ret;
// sin(theta/2) = 2*tan(theta/4) / (1 + tan^2(theta/4))
// v == v.normalized() * tan^2(theta/4)
ret.vec() = v*(2/(1+a2));
// cos(theta/2) = (1 - tan^2(theta/4)) / (1 + tan^2(theta/4))
ret.w() = (1-a2)/(1+a2);
return ret;
}
template<typename FloatT>
Eigen::Matrix<FloatT, 3, 1> log_r(const Quaternion<FloatT>& q)
{
// Note: This algorithm is reasonably safe when using double
// precision (to within 1e-10 of precision), but not float.
/*
* q.w() == cos(theta/2)
* q.vec() == sin(theta/2)*v_hat
*
*
* Normal rodrigues params:
* v*tan(theta/2) = v*sin(theta/2) / cos(theta/2)
* v*tan(theta/2) = q.vec() / q.w()
*
* Modified rodrigues params (pulls away from infinity at theta=pi)
* tan(theta/2) == sin(theta) / (1 + cos(theta))
* therefore, tan(theta/4)*v_hat = sin(theta/2)*v_hat / (1 + cos(theta/2))
*/
return q.vec() / (1.0+q.w());
}
/**
* Convert an angle/axis 3-vector to a unit quaternion
* @param v A 3-vector whose length is between 0 and 2*pi
* @return The quaternion that represents the same rotation.
*/
template<typename FloatT>
Quaternion<FloatT> exp(Eigen::Matrix<FloatT, 3, 1> v);
template<typename FloatT>
Quaternion<FloatT> exp(Eigen::Matrix<FloatT, 3, 1> v)
{
FloatT angle = v.norm();
if (angle <= Eigen::machine_epsilon<FloatT>()) {
// std::cerr << "Warning: tiny quaternion flushed to zero\n";
return Quaternion<FloatT>::Identity();
}
else {
Quaternion<FloatT> ret;
#if 0
if (angle > 1.999*M_PI) {
// TODO: I really, really don't like this hack. It should
// be impossible to compute an angular measurement update
// with a rotation angle greater than this number...
v *= 1.999*M_PI / angle;
angle = 1.999*M_PI;
}
#endif
assert(angle <= FloatT(2.0*M_PI));
#if 0
// Oddly enough, this attempt to make the formula faster by reducing
// the number of trig calls actually runs slower.
FloatT tan_x = std::tan(angle * 0.25);
FloatT cos_angle = (1 - tan_x*tan_x)/(1+tan_x*tan_x);
FloatT sin_angle = 2*tan_x/(1+tan_x*tan_x);
ret.w() = cos_angle;
ret.vec() = (sin_angle/angle)*v;
#else
ret.w() = std::cos(angle*0.5);
ret.vec() = (std::sin(angle*0.5)/angle)*v;
#endif
return ret;
// return Quaternion<FloatT>(Eigen::AngleAxis<FloatT>(angle, v / angle));
}
}
/**
* Convert a unit quaternion to multiplied angle/axis form.
* @param q A unit quaternion. The quaternion's norm should be close to unity,
* but may be slightly too large or small.
* @return The 3-vector in the tangent space of the quaternion q.
*/
template<typename FloatT>
Eigen::Matrix<FloatT, 3, 1> log(const Quaternion<FloatT>& q) __attribute__((noinline));
template<typename FloatT>
Eigen::Matrix<FloatT, 3, 1> log(const Quaternion<FloatT>& q)
{
FloatT mag = q.vec().norm();
if (mag <= Eigen::machine_epsilon<FloatT>()) {
// Flush to zero for very small angles. This avoids division by zero.
return Eigen::Matrix<FloatT, 3, 1>::Zero();
}
FloatT angle = 2.0*std::atan2(mag, q.w());
return q.vec() * (angle/mag);
// Eigen::AngleAxis<FloatT> res(q /*mag <= 1.0) ? q : q.normalized() */);
// return res.axis() * res.angle();
}
#endif
#ifndef AHRS_QUATERNIONS_HPP
#define AHRS_QUATERNIONS_HPP
/*
* quaternions.cpp
*
* Author: Jonathan Brandmeyer
*
* This file is part of libeknav.
*
* Libeknav is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, version 3.
*
* Libeknav is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*/
#include <Eigen/Core>
#include <Eigen/Geometry>
using Eigen::Quaternion;
template<typename FloatT>
Quaternion<FloatT> operator-(const Quaternion<FloatT>& q)
{
return Quaternion<FloatT>(-q.w(), -q.x(), -q.y(), -q.z());
}
/**
* Convert a rotation from modified Rodrigues parameters to a quaternion.
* @param v The multiplication of the rotation angle by the tangent of the angle/4
* @return The quaternion corresponding to that rotation.
*/
template<typename FloatT>
Quaternion<FloatT> exp_r(const Eigen::Matrix<FloatT, 3, 1>& v)
{
// a2 = tan^2(theta/4)
FloatT a2 = v.squaredNorm();
Quaternion<FloatT> ret;
// sin(theta/2) = 2*tan(theta/4) / (1 + tan^2(theta/4))
// v == v.normalized() * tan^2(theta/4)
ret.vec() = v*(2/(1+a2));
// cos(theta/2) = (1 - tan^2(theta/4)) / (1 + tan^2(theta/4))
ret.w() = (1-a2)/(1+a2);
return ret;
}
template<typename FloatT>
Eigen::Matrix<FloatT, 3, 1> log_r(const Quaternion<FloatT>& q)
{
// Note: This algorithm is reasonably safe when using double
// precision (to within 1e-10 of precision), but not float.
/*
* q.w() == cos(theta/2)
* q.vec() == sin(theta/2)*v_hat
*
*
* Normal rodrigues params:
* v*tan(theta/2) = v*sin(theta/2) / cos(theta/2)
* v*tan(theta/2) = q.vec() / q.w()
*
* Modified rodrigues params (pulls away from infinity at theta=pi)
* tan(theta/2) == sin(theta) / (1 + cos(theta))
* therefore, tan(theta/4)*v_hat = sin(theta/2)*v_hat / (1 + cos(theta/2))
*/
return q.vec() / (1.0+q.w());
}
/**
* Convert an angle/axis 3-vector to a unit quaternion
* @param v A 3-vector whose length is between 0 and 2*pi
* @return The quaternion that represents the same rotation.
*/
template<typename FloatT>
Quaternion<FloatT> exp(Eigen::Matrix<FloatT, 3, 1> v);
template<typename FloatT>
Quaternion<FloatT> exp(Eigen::Matrix<FloatT, 3, 1> v)
{
FloatT angle = v.norm();
if (angle <= Eigen::machine_epsilon<FloatT>()) {
// std::cerr << "Warning: tiny quaternion flushed to zero\n";
return Quaternion<FloatT>::Identity();
}
else {
Quaternion<FloatT> ret;
#if 0
if (angle > 1.999*M_PI) {
// TODO: I really, really don't like this hack. It should
// be impossible to compute an angular measurement update
// with a rotation angle greater than this number...
v *= 1.999*M_PI / angle;
angle = 1.999*M_PI;
}
#endif
assert(angle <= FloatT(2.0*M_PI));
#if 0
// Oddly enough, this attempt to make the formula faster by reducing
// the number of trig calls actually runs slower.
FloatT tan_x = std::tan(angle * 0.25);
FloatT cos_angle = (1 - tan_x*tan_x)/(1+tan_x*tan_x);
FloatT sin_angle = 2*tan_x/(1+tan_x*tan_x);
ret.w() = cos_angle;
ret.vec() = (sin_angle/angle)*v;
#else
ret.w() = std::cos(angle*0.5);
ret.vec() = (std::sin(angle*0.5)/angle)*v;
#endif
return ret;
// return Quaternion<FloatT>(Eigen::AngleAxis<FloatT>(angle, v / angle));
}
}
/**
* Convert a unit quaternion to multiplied angle/axis form.
* @param q A unit quaternion. The quaternion's norm should be close to unity,
* but may be slightly too large or small.
* @return The 3-vector in the tangent space of the quaternion q.
*/
template<typename FloatT>
Eigen::Matrix<FloatT, 3, 1> log(const Quaternion<FloatT>& q) __attribute__((noinline));
template<typename FloatT>
Eigen::Matrix<FloatT, 3, 1> log(const Quaternion<FloatT>& q)
{
FloatT mag = q.vec().norm();
if (mag <= Eigen::machine_epsilon<FloatT>()) {
// Flush to zero for very small angles. This avoids division by zero.
return Eigen::Matrix<FloatT, 3, 1>::Zero();
}
FloatT angle = 2.0*std::atan2(mag, q.w());
return q.vec() * (angle/mag);
// Eigen::AngleAxis<FloatT> res(q /*mag <= 1.0) ? q : q.normalized() */);
// return res.axis() * res.angle();
}
#endif
sw/in_progress/airspeed/rls.m
+40 -40
View File
@@ -1,40 +1,40 @@
clear
close all
% DATA
t=1:200;
orig = sin(t./3)+sin(t./100.*2.*pi);
echo = [orig(4:end) 0 0 0]./1.2 + 0.5;
echo = echo + randn(size(orig))./10;
x = echo;
d = orig;
% RLS
rls_p = 5;
rls_delta = 1;
rls_lambda = 0.95;
rls_x = ones(rls_p+1, 1).* x(1);
rls_w = ones(rls_p+1, 1)./(rls_p+1);
rls_P = eye(rls_p+1) .* rls_delta;
filt = x;
for i=t
rls_x = [ x(i); rls_x(1:end-2); 1];
filt(i) = rls_w' * rls_x;
rls_alpha = d(i) - filt(i);
rls_g = rls_P * rls_x / (rls_lambda + rls_x' * rls_P * rls_x);
rls_P = rls_P / rls_lambda - rls_g*rls_x'* rls_P / rls_lambda;
rls_w = rls_w + rls_alpha * rls_g;
end
close all;
figure;
plot(orig,'r');
hold on;
plot(echo,'b');
plot(filt,'g');
clear
close all
% DATA
t=1:200;
orig = sin(t./3)+sin(t./100.*2.*pi);
echo = [orig(4:end) 0 0 0]./1.2 + 0.5;
echo = echo + randn(size(orig))./10;
x = echo;
d = orig;
% RLS
rls_p = 5;
rls_delta = 1;
rls_lambda = 0.95;
rls_x = ones(rls_p+1, 1).* x(1);
rls_w = ones(rls_p+1, 1)./(rls_p+1);
rls_P = eye(rls_p+1) .* rls_delta;
filt = x;
for i=t
rls_x = [ x(i); rls_x(1:end-2); 1];
filt(i) = rls_w' * rls_x;
rls_alpha = d(i) - filt(i);
rls_g = rls_P * rls_x / (rls_lambda + rls_x' * rls_P * rls_x);
rls_P = rls_P / rls_lambda - rls_g*rls_x'* rls_P / rls_lambda;
rls_w = rls_w + rls_alpha * rls_g;
end
close all;
figure;
plot(orig,'r');
hold on;
plot(echo,'b');
plot(filt,'g');
sw/in_progress/airspeed/rls_control.m
+99 -99
View File
@@ -1,99 +1,99 @@
clear
close all
% DATA
tmax = 200;
t=1:tmax;
command = zeros(1,tmax);
command(50:150) = 1;
% reference model
vref = zeros(1,tmax);
vrefdot = 0;
sysx = 0.2;
sysvx = 0;
plotsys = zeros(1,tmax);
integrator = 0;
plotint =zeros(1,tmax);
x = zeros(1,1:tmax);
d = zeros(1,1:tmax);
filt = zeros(1,1:tmax);
% RLS
rls_p = 5;
rls_delta = 1;
rls_lambda = 0.95;
rls_x = ones(rls_p+1, 1).* x(1);
rls_w = ones(rls_p+1, 1)./(rls_p+1);
rls_P = eye(rls_p+1) .* rls_delta;
for i=2:tmax
acc = (command(i) - vref(i-1)) / 8 - vrefdot * 0.7 ;
if (acc > 0.02)
acc = 0.02;
elseif (acc<-0.02)
acc = -0.02;
end
vrefdot = vrefdot + acc;
if vrefdot > 0.5
vrefdot = 0.5
elseif vrefdot < -0.5
vrefdot = 0.5
end
vref(i) = vref(i-1) + vrefdot;
% Control with Integrator
error = vref(i) - sysx + rand(1,1) / 20.0;
integrator = integrator + error / 2;
syscomm = error * 4 + integrator;
sysacc = (syscomm - (sysx-0.2)*0.75) / 5 - sysvx*0.6;
if (sysacc > 0.04)
sysacc = 0.04;
elseif (sysacc<-0.04)
sysacc = -0.04;
end
sysvx = sysvx + sysacc;
if sysvx > 0.6
sysvx = 0.6
elseif sysvx < -0.7
sysvx = 0.7
end
sysx = sysx + sysvx;
plotsys(i) = sysx;
plotint(i) = integrator;
end
close all
figure
plot(command , 'r');
hold on
plot(vref,'g');
plot(plotsys,'b')
plot(plotint,'k');
%% Rest
for i=t
rls_x = [ x(i); rls_x(1:end-1)];
filt(i) = rls_w' * rls_x;
rls_alpha = d(i) - filt(i);
rls_g = rls_P * rls_x / (rls_lambda + rls_x' * rls_P * rls_x);
rls_P = rls_P / rls_lambda - rls_g*rls_x'* rls_P / rls_lambda;
rls_w = rls_w + rls_alpha * rls_g;
end
figure;
plot(orig,'r');
hold on;
plot(echo,'b');
plot(filt,'g');
clear
close all
% DATA
tmax = 200;
t=1:tmax;
command = zeros(1,tmax);
command(50:150) = 1;
% reference model
vref = zeros(1,tmax);
vrefdot = 0;
sysx = 0.2;
sysvx = 0;
plotsys = zeros(1,tmax);
integrator = 0;
plotint =zeros(1,tmax);
x = zeros(1,1:tmax);
d = zeros(1,1:tmax);
filt = zeros(1,1:tmax);
% RLS
rls_p = 5;
rls_delta = 1;
rls_lambda = 0.95;
rls_x = ones(rls_p+1, 1).* x(1);
rls_w = ones(rls_p+1, 1)./(rls_p+1);
rls_P = eye(rls_p+1) .* rls_delta;
for i=2:tmax
acc = (command(i) - vref(i-1)) / 8 - vrefdot * 0.7 ;
if (acc > 0.02)
acc = 0.02;
elseif (acc<-0.02)
acc = -0.02;
end
vrefdot = vrefdot + acc;
if vrefdot > 0.5
vrefdot = 0.5
elseif vrefdot < -0.5
vrefdot = 0.5
end
vref(i) = vref(i-1) + vrefdot;
% Control with Integrator
error = vref(i) - sysx + rand(1,1) / 20.0;
integrator = integrator + error / 2;
syscomm = error * 4 + integrator;
sysacc = (syscomm - (sysx-0.2)*0.75) / 5 - sysvx*0.6;
if (sysacc > 0.04)
sysacc = 0.04;
elseif (sysacc<-0.04)
sysacc = -0.04;
end
sysvx = sysvx + sysacc;
if sysvx > 0.6
sysvx = 0.6
elseif sysvx < -0.7
sysvx = 0.7
end
sysx = sysx + sysvx;
plotsys(i) = sysx;
plotint(i) = integrator;
end
close all
figure
plot(command , 'r');
hold on
plot(vref,'g');
plot(plotsys,'b')
plot(plotint,'k');
%% Rest
for i=t
rls_x = [ x(i); rls_x(1:end-1)];
filt(i) = rls_w' * rls_x;
rls_alpha = d(i) - filt(i);
rls_g = rls_P * rls_x / (rls_lambda + rls_x' * rls_P * rls_x);
rls_P = rls_P / rls_lambda - rls_g*rls_x'* rls_P / rls_lambda;
rls_w = rls_w + rls_alpha * rls_g;
end
figure;
plot(orig,'r');
hold on;
plot(echo,'b');
plot(filt,'g');
sw/in_progress/airspeed/ultrasimplesim.m
+429 -429
View File
File diff suppressed because it is too large Load Diff
sw/in_progress/videolizer/wis-go7007-linux/firmware/ezusb/hpi_PX-TV402U.hex
+368 -368
View File
File diff suppressed because it is too large Load Diff
sw/in_progress/videolizer/wis-go7007-linux/firmware/ezusb/hpi_StarTrek.hex
+358 -358
View File
File diff suppressed because it is too large Load Diff
sw/tools/calibration/airspeed.m
+75 -75
View File
@@ -1,75 +1,75 @@
close all
%% Read Data
% AIR = load('airspeed.txt');
% air = AIR(:,5);
% desired = AIR(:,6);
% climb = AIR(:,7)./100;
% speed = AIR(:,8)./100;
% alt = AIR(:,9)./100;
% course = AIR(:,10)./10;
%
% airspeed_bias = 64.5;
% airspeed_scale = 3.5;
%% Read data
AIR = load('airspeed_amp.txt');
AIR = AIR(1000:end,:);
air = AIR(:,6);
desired = AIR(:,9)./100;
climb = AIR(:,7)./100;
speed = AIR(:,8)./100;
alt = AIR(:,9)./100;
course = AIR(:,10)./10;
airspeed_bias = 370;
airspeed_scale = 3.5 / 5.7 * 2.4;
%% Altitude
figure
hold on
plot (alt);
plot(desired,'r');
grid on;
%% Wind
windx = 7;
windy = 0;
figure;
vx = speed .* cosd(course) + windx;
vy = speed .* sind(course) + windy;
vz = climb;
plot(vx,vy,'bx');
grid on;
axis equal;
%% Airspeed Calibration
gpsairspeed = sqrt(vx.^2 + vy .^2 + vz.^2);
airsp = air - airspeed_bias;
airsp = airsp - airsp .* (airsp < 0);
airsp = sqrt(airsp) * airspeed_scale;
% [b,a] = butter(2,0.1);
% airsp = filter(b,a,airsp);
% gpsairspeed = filter(b,a,gpsairspeed);
figure
plot(airsp,'b');
hold on;
plot(gpsairspeed,'r');
grid on;
legend('air','gps');
%plot(climb,'g');
%% Total Energy
%Ekin = airsp .*
close all
%% Read Data
% AIR = load('airspeed.txt');
% air = AIR(:,5);
% desired = AIR(:,6);
% climb = AIR(:,7)./100;
% speed = AIR(:,8)./100;
% alt = AIR(:,9)./100;
% course = AIR(:,10)./10;
%
% airspeed_bias = 64.5;
% airspeed_scale = 3.5;
%% Read data
AIR = load('airspeed_amp.txt');
AIR = AIR(1000:end,:);
air = AIR(:,6);
desired = AIR(:,9)./100;
climb = AIR(:,7)./100;
speed = AIR(:,8)./100;
alt = AIR(:,9)./100;
course = AIR(:,10)./10;
airspeed_bias = 370;
airspeed_scale = 3.5 / 5.7 * 2.4;
%% Altitude
figure
hold on
plot (alt);
plot(desired,'r');
grid on;
%% Wind
windx = 7;
windy = 0;
figure;
vx = speed .* cosd(course) + windx;
vy = speed .* sind(course) + windy;
vz = climb;
plot(vx,vy,'bx');
grid on;
axis equal;
%% Airspeed Calibration
gpsairspeed = sqrt(vx.^2 + vy .^2 + vz.^2);
airsp = air - airspeed_bias;
airsp = airsp - airsp .* (airsp < 0);
airsp = sqrt(airsp) * airspeed_scale;
% [b,a] = butter(2,0.1);
% airsp = filter(b,a,airsp);
% gpsairspeed = filter(b,a,gpsairspeed);
figure
plot(airsp,'b');
hold on;
plot(gpsairspeed,'r');
grid on;
legend('air','gps');
%plot(climb,'g');
%% Total Energy
%Ekin = airsp .*