mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-27 08:55:51 +08:00
rename in_progress dir to misc
This commit is contained in:
Felix Ruess
@@ -0,0 +1,121 @@
|
||||
#
|
||||
# $Id: Makefile 5874 2010-09-15 03:41:04Z aibara $
|
||||
# Copyright (C) 2004 Pascal Brisset, Antoine Drouin
|
||||
#
|
||||
# This file is part of paparazzi.
|
||||
#
|
||||
# paparazzi 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; either version 2, or (at your option)
|
||||
# any later version.
|
||||
#
|
||||
# paparazzi 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 paparazzi; see the file COPYING. If not, write to
|
||||
# the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
# Boston, MA 02111-1307, USA.
|
||||
#
|
||||
|
||||
# Quiet compilation
|
||||
Q=@
|
||||
|
||||
OCAMLC = ocamlc
|
||||
OCAMLOPT = ocamlopt
|
||||
INCLUDES= $(shell ocamlfind query -r -i-format xml-light) $(shell ocamlfind query -r -i-format lablgtk2) -I ../../lib/ocaml
|
||||
|
||||
all: panic
|
||||
|
||||
$^
|
||||
|
||||
|
||||
play play-nox plotter sd2log : ../../lib/ocaml/lib-pprz.cma
|
||||
plot : ../../lib/ocaml/lib-pprz.cmxa
|
||||
|
||||
# Target for bytecode executable (if ocamlopt is not available)
|
||||
# plot : log_file.cmo gtk_export.cmo export.cmo plot.cmo
|
||||
# @echo OL $@
|
||||
# $(Q)$(OCAMLC) $(INCLUDES) -o $@ unix.cma str.cma xml-light.cma glibivy-ocaml.cma lablgtk.cma lib-pprz.cma lablglade.cma gtkInit.cmo $^
|
||||
|
||||
%.cmo: %.ml
|
||||
@echo OC $<
|
||||
$(Q)$(OCAMLC) $(OCAMLCFLAGS) $(INCLUDES) -c $<
|
||||
%.cmi: %.mli
|
||||
@echo OCI $<
|
||||
$(Q)$(OCAMLC) $(OCAMLCFLAGS) $(INCLUDES) -c $<
|
||||
%.cmx: %.ml
|
||||
@echo OOC $<
|
||||
$(Q)$(OCAMLOPT) $(OCAMLCFLAGS) $(INCLUDES) -c $<
|
||||
|
||||
export.cmo : gtk_export.cmo
|
||||
export.cmx : gtk_export.cmx
|
||||
|
||||
UNAME = $(shell uname -s)
|
||||
ifeq ("$(UNAME)","Darwin")
|
||||
MKTEMP = gmktemp
|
||||
else
|
||||
MKTEMP = mktemp
|
||||
endif
|
||||
|
||||
gtk_export.ml : export.glade
|
||||
@echo GLADE $@
|
||||
$(eval $@_TMP := $(shell $(MKTEMP)))
|
||||
$(Q)grep -v invisible_char $< > $($@_TMP)
|
||||
$(Q)lablgladecc2 -root export -hide-default $($@_TMP) | grep -B 1000000 " end" > $@
|
||||
$(Q)rm -f $($@_TMP)
|
||||
|
||||
pt : ahrsview imuview ahrs2fg
|
||||
|
||||
CC = gcc
|
||||
CFLAGS=-g -O2 -Wall $(shell pkg-config gtk+-2.0 --cflags)
|
||||
LDFLAGS=$(shell pkg-config gtk+-2.0 --libs) -s -lgtkdatabox -lglibivy
|
||||
|
||||
MORE_FLAGS = -I/usr/include/gtk-1.2 -I/usr/include/glib-1.2 -I/usr/lib/glib/include -rdynamic /usr/lib/libgtkgl.so -L/usr/lib -L/usr/X11R6/lib /usr/lib/libgtk.so /usr/lib/libgdk.so /usr/lib/libgmodule.so /usr/lib/libglib.so -ldl -lXi -lXext -lX11 -lm -lGLU -lGL -Wl,--rpath -Wl,/usr/local/lib -lglibivy $(shell pcre-config --libs)
|
||||
|
||||
MORE_CFLAGS = -DHAVE_DLFCN_H=1 -DSTDC_HEADERS=1 -I. -I. -I.. -g -O2 -I/usr/include/gtk-1.2 -I/usr/include/glib-1.2 -I/usr/lib/glib/include
|
||||
|
||||
clean:
|
||||
$(Q)rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* panic
|
||||
|
||||
#FGFS_PREFIX=/home/poine/local
|
||||
FGFS_PREFIX=/home/poine/flightgear
|
||||
#FGFS_PREFIX=/usr/local
|
||||
|
||||
#FGFS_ROOT = /home/poine/local
|
||||
FGFS_ROOT = /home/poine/flightgear
|
||||
FGFS = $(FGFS_PREFIX)/bin/fgfs
|
||||
#FGFS = /usr/games/fgfs
|
||||
#FGFS_ENV = LD_LIBRARY_PATH=/usr/local/lib:$(FGFS_ROOT)/lib
|
||||
FGFS_ENV = LD_LIBRARY_PATH=$(FGFS_ROOT)/lib
|
||||
FGFS_COMMON_ARGS = --fg-root=$(FGFS_ROOT) --aircraft=A320 --timeofday=noon
|
||||
#FGFS_COMMON_ARGS = --aircraft=737-300 --timeofday=noon
|
||||
FGFS_IN_FDM_ARGS = $(FGFS_COMMON_ARGS) --fdm=null --native-fdm=socket,in,30,,5501,udp
|
||||
FGFS_OUT_FDM_ARGS = $(FGFS_COMMON_ARGS) --native-fdm=socket,out,30,,5500,udp
|
||||
FGFS_IN_GUI_ARGS = $(FGFS_COMMON_ARGS) --fdm=null --native-gui=socket,in,30,,5501,udp
|
||||
FGFS_OUT_GUI_ARGS = $(FGFS_COMMON_ARGS) --native-gui=socket,out,30,,5500,udp
|
||||
FGFS_IN_CTRLS_ARGS = $(FGFS_COMMON_ARGS) --native-ctrls=socket,in,30,,5501,udp
|
||||
FGFS_OUT_CTRLS_ARGS = $(FGFS_COMMON_ARGS) --native-ctrls=socket,out,30,,5500,udp
|
||||
FGFS_OUT_MULTIPLAY_ARGS = $(FGFS_COMMON_ARGS) --multiplay=out,10,127.0.0.1,5500 --callsign=F-POINE
|
||||
FGFS_IN_MULTIPLAY_ARGS = $(FGFS_COMMON_ARGS) --multiplay=in,10,127.0.0.1,5501 --callsign=F-POINE
|
||||
FGFS_RCAM_ARGS = $(FGFS_COMMON_ARGS) --fdm=null --native-ctrls=socket,out,30,,5500,udp --native-fdm=socket,in,30,,5501,udp
|
||||
FGFS_GAME_ARGS = $(FGFS_COMMON_ARGS) --control=joystick
|
||||
|
||||
FGFS_ARGS = $(FGFS_COMMON_ARGS) $(FGFS_IN_GUI_ARGS)
|
||||
#FGFS_GAME_ARGS)
|
||||
|
||||
panic: panic.c
|
||||
gcc -g -O2 -Wall $(shell pkg-config glib-2.0 --cflags) -o $@ $^ $(shell pkg-config glib-2.0 --libs) -lglibivy -lhid
|
||||
|
||||
#
|
||||
# Dependencies
|
||||
#
|
||||
|
||||
.depend: Makefile
|
||||
ocamldep -I ../../lib/ocaml *.ml* > .depend
|
||||
|
||||
ifneq ($(MAKECMDGOALS),clean)
|
||||
-include .depend
|
||||
endif
|
||||
@@ -0,0 +1,155 @@
|
||||
/*==============================================
|
||||
PoC for USB Panic Button under unix
|
||||
--------------------------------------------
|
||||
by: Benjamin Kendinibilir
|
||||
needs: libusb and libhid
|
||||
compile: gcc usbpanicbutton.c -o upb -lhid
|
||||
run: sudo ./upb (needs root)
|
||||
==============================================*/
|
||||
|
||||
#include <hid.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <signal.h>
|
||||
#include <glib.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#define VENDOR_ID 0x1130
|
||||
#define PRODUCT_ID 0x0202
|
||||
#define IFACE_NO 0
|
||||
#define PACKET_SIZE 8
|
||||
#define PATH1 0x00010000
|
||||
#define PATH2 0x00000000
|
||||
#define PATH_LEN 2
|
||||
|
||||
#define TIMEOUT_PERIOD 20
|
||||
|
||||
int loop = 1;
|
||||
hid_return ret;
|
||||
HIDInterface* hid;
|
||||
char packet[PACKET_SIZE];
|
||||
int const path_out[] = { PATH1, PATH2 };
|
||||
HIDInterfaceMatcher matcher = { VENDOR_ID, PRODUCT_ID, NULL, NULL, 0 };
|
||||
GMainLoop *ml;
|
||||
int ac_id, block_nr;
|
||||
|
||||
void endloop(int signum) {
|
||||
loop = 0;
|
||||
g_main_loop_quit(ml);
|
||||
}
|
||||
|
||||
int btn_init(void) {
|
||||
|
||||
#ifdef DEBUG
|
||||
hid_set_debug(HID_DEBUG_ALL);
|
||||
hid_set_debug_stream(stderr);
|
||||
hid_set_usb_debug(0);
|
||||
#endif
|
||||
|
||||
ret = hid_init();
|
||||
if (ret != HID_RET_SUCCESS) {
|
||||
fprintf(stderr, "hid_init failed with return code %d\n", ret);
|
||||
return 1;
|
||||
}
|
||||
|
||||
hid = hid_new_HIDInterface();
|
||||
if (hid == 0) {
|
||||
fprintf(stderr, "hid_new_HIDInterface() failed, out of memory?\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
ret = hid_force_open(hid, IFACE_NO, &matcher, 3);
|
||||
if (ret != HID_RET_SUCCESS) {
|
||||
fprintf(stderr, "hid_force_open failed with return code %d\n", ret);
|
||||
return 1;
|
||||
}
|
||||
|
||||
#ifdef DEBUG
|
||||
ret = hid_write_identification(stdout, hid);
|
||||
if (ret != HID_RET_SUCCESS) {
|
||||
fprintf(stderr, "hid_write_identification failed with return code %d\n", ret);
|
||||
return 1;
|
||||
}
|
||||
|
||||
ret = hid_dump_tree(stdout, hid);
|
||||
if (ret != HID_RET_SUCCESS) {
|
||||
fprintf(stderr, "hid_dump_tree failed with return code %d\n", ret);
|
||||
return 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
signal(SIGINT, endloop);
|
||||
signal(SIGHUP, endloop);
|
||||
signal(SIGTERM, endloop);
|
||||
|
||||
printf("waiting for panic button action:\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static gboolean btn_periodic(gpointer data __attribute__ ((unused))) {
|
||||
|
||||
ret = hid_get_input_report(hid, path_out, PATH_LEN, packet, PACKET_SIZE);
|
||||
if (ret != HID_RET_SUCCESS) {
|
||||
fprintf(stderr, "hid_get_input_report failed with return code %d\n", ret);
|
||||
}
|
||||
|
||||
if(packet[0] == 0x1) {
|
||||
IvySendMsg("dl JUMP_TO_BLOCK %d %d", ac_id, block_nr);
|
||||
printf("\aplop\n");
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
return loop;
|
||||
}
|
||||
|
||||
int btn_close(void) {
|
||||
|
||||
printf("cleaning up... ");
|
||||
|
||||
ret = hid_close(hid);
|
||||
if (ret != HID_RET_SUCCESS) {
|
||||
fprintf(stderr, "hid_close failed with return code %d\n", ret);
|
||||
return 1;
|
||||
}
|
||||
|
||||
hid_delete_HIDInterface(&hid);
|
||||
|
||||
ret = hid_cleanup();
|
||||
if (ret != HID_RET_SUCCESS) {
|
||||
fprintf(stderr, "hid_cleanup failed with return code %d\n", ret);
|
||||
return 1;
|
||||
}
|
||||
|
||||
printf("exit.\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
if (argc != 3) {
|
||||
printf("usage: panic <ac_id> <block_nr>\n");
|
||||
return 1;
|
||||
}
|
||||
ac_id = strtol(argv[1], NULL, 10);
|
||||
block_nr = strtol(argv[2], NULL, 10);
|
||||
|
||||
ml = g_main_loop_new(NULL, FALSE);
|
||||
|
||||
IvyInit ("IvyCtrlButton", "IvyCtrlButton READY", NULL, NULL, NULL, NULL);
|
||||
IvyStart("127.255.255.255");
|
||||
|
||||
btn_init();
|
||||
|
||||
g_timeout_add(TIMEOUT_PERIOD, btn_periodic, NULL);
|
||||
g_main_loop_run(ml);
|
||||
|
||||
btn_close();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
#ifndef _6DOF_H
|
||||
#define _6DOF_H
|
||||
|
||||
#define AXIS_X 0
|
||||
#define AXIS_Y 1
|
||||
#define AXIS_Z 2
|
||||
#define AXIS_NB 3
|
||||
|
||||
#define AXIS_P 0
|
||||
#define AXIS_Q 1
|
||||
#define AXIS_R 2
|
||||
|
||||
#endif /* _6DOF_H */
|
||||
@@ -0,0 +1,133 @@
|
||||
|
||||
# Quiet compilation
|
||||
Q=@
|
||||
|
||||
CFLAGS = -g -Wall $(shell pkg-config glib-2.0 --cflags)
|
||||
LDFLAGS = $(shell pkg-config glib-2.0 --libs) -lm
|
||||
|
||||
%.o: %.c
|
||||
gcc -c -Wall $(CFLAGS) -o $@ $<
|
||||
|
||||
CFLAGS += $(shell pkg-config gtk+-2.0 --cflags)
|
||||
LDFLAGS += $(shell pkg-config gtk+-2.0 --libs) -lgtkdatabox
|
||||
|
||||
#
|
||||
#
|
||||
# TILT
|
||||
#
|
||||
#
|
||||
|
||||
OBJS_TILT_UKF = tilt_data.o \
|
||||
tilt_display.o \
|
||||
tilt_utils.o \
|
||||
tilt_ukf.o \
|
||||
random.c \
|
||||
ukf.c \
|
||||
linalg.c \
|
||||
|
||||
tilt_ukf: $(OBJS_TILT_UKF)
|
||||
gcc -o $@ $^ $(LDFLAGS)
|
||||
|
||||
OBJS_TILT_EKF = tilt_data.o \
|
||||
tilt_display.o \
|
||||
tilt_utils.o \
|
||||
tilt_ekf.o \
|
||||
random.o \
|
||||
ekf.o \
|
||||
matrix.o \
|
||||
|
||||
#CFLAGS += -DEKF_UPDATE_DISCRETE
|
||||
CFLAGS += -DEKF_UPDATE_CONTINUOUS
|
||||
tilt_ekf: $(OBJS_TILT_EKF)
|
||||
gcc -o $@ $^ $(LDFLAGS)
|
||||
|
||||
|
||||
OBJS_TILT_FAST_EKF = tilt_data.o \
|
||||
tilt_display.o \
|
||||
tilt_utils.o \
|
||||
random.o \
|
||||
tilt_fast_ekf.o
|
||||
|
||||
tilt_fast_ekf: $(OBJS_TILT_FAST_EKF)
|
||||
gcc -o $@ $^ $(LDFLAGS)
|
||||
|
||||
|
||||
#
|
||||
#
|
||||
# AHRS EULER
|
||||
#
|
||||
#
|
||||
|
||||
OBJS_AHRS_EULER_EKF = ahrs_euler_ekf.o \
|
||||
ahrs_utils.o \
|
||||
ahrs_data.o \
|
||||
ahrs_display.o \
|
||||
ekf.o \
|
||||
matrix.o
|
||||
|
||||
ahrs_euler_ekf: $(OBJS_AHRS_EULER_EKF)
|
||||
gcc -o $@ $^ $(LDFLAGS)
|
||||
|
||||
|
||||
|
||||
#
|
||||
#
|
||||
# AHRS QUAT
|
||||
#
|
||||
#
|
||||
|
||||
OBJS_AHRS_QUAT_UKF = ahrs_quat_ukf.o \
|
||||
ahrs_utils.o \
|
||||
ahrs_data.o \
|
||||
ahrs_display.o \
|
||||
ukf.o \
|
||||
linalg.o
|
||||
|
||||
ahrs_quat_ukf: $(OBJS_AHRS_QUAT_UKF)
|
||||
gcc -o $@ $^ $(LDFLAGS)
|
||||
|
||||
OBJS_AHRS_QUAT_EKF = ahrs_quat_ekf.o \
|
||||
ahrs_utils.o \
|
||||
ahrs_data.o \
|
||||
ahrs_display.o \
|
||||
ekf.o \
|
||||
matrix.o
|
||||
|
||||
ahrs_quat_ekf: $(OBJS_AHRS_QUAT_EKF)
|
||||
gcc -o $@ $^ $(LDFLAGS)
|
||||
|
||||
|
||||
OBJS_AHRS_QUAT_FAST_EKF = ahrs_quat_fast_ekf_main.o \
|
||||
ahrs_quat_fast_ekf.o \
|
||||
ahrs_data.o \
|
||||
ahrs_display.o \
|
||||
ahrs_utils.o \
|
||||
|
||||
|
||||
ahrs_quat_fast_ekf: $(OBJS_AHRS_QUAT_FAST_EKF)
|
||||
gcc -o $@ $^ $(LDFLAGS)
|
||||
|
||||
|
||||
#
|
||||
#
|
||||
# Test
|
||||
#
|
||||
#
|
||||
|
||||
OBJS_TEST_MATRIX = test_matrix.o \
|
||||
matrix.o
|
||||
|
||||
test_matrix: $(OBJS_TEST_MATRIX)
|
||||
gcc -o $@ $^ $(LDFLAGS)
|
||||
|
||||
OBJS_TEST_UKF = main.o \
|
||||
linalg.o \
|
||||
random.o \
|
||||
ukf.o \
|
||||
|
||||
test_ukf: $(OBJS_TEST_UKF)
|
||||
gcc -o $@ $^ $(LDFLAGS)
|
||||
|
||||
|
||||
clean:
|
||||
$(Q)rm -f *~ *.o tilt_ukf tilt_ekf tilt_fast_ekf ahrs_euler_ekf ahrs_quat_ukf ahrs_quat_ekf ahrs_quat_fast_ekf test_matrix test_ukf
|
||||
@@ -0,0 +1,190 @@
|
||||
#include "ahrs_data.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <glib.h>
|
||||
|
||||
#include "random.h"
|
||||
#include "ahrs_utils.h"
|
||||
|
||||
struct ahrs_data* ahrs_data_new(int len, double dt) {
|
||||
|
||||
struct ahrs_data* ad = malloc(sizeof(struct ahrs_data));
|
||||
ad->dt = dt;
|
||||
ad->nb_samples = len;
|
||||
|
||||
ad->t = malloc(ad->nb_samples*sizeof(double));
|
||||
|
||||
ad->phi = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->theta = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->psi = malloc(ad->nb_samples*sizeof(double));
|
||||
|
||||
ad->bias_p = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->bias_q = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->bias_r = malloc(ad->nb_samples*sizeof(double));
|
||||
|
||||
|
||||
ad->gyro_p = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->gyro_q = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->gyro_r = malloc(ad->nb_samples*sizeof(double));
|
||||
|
||||
ad->accel_x = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->accel_y = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->accel_z = malloc(ad->nb_samples*sizeof(double));
|
||||
|
||||
ad->mag_x = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->mag_y = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->mag_z = malloc(ad->nb_samples*sizeof(double));
|
||||
|
||||
ad->m_phi = malloc(ad->nb_samples*sizeof(double));;
|
||||
ad->m_theta = malloc(ad->nb_samples*sizeof(double));;
|
||||
ad->m_psi = malloc(ad->nb_samples*sizeof(double));;
|
||||
|
||||
ad->est_phi = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->est_theta = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->est_psi = malloc(ad->nb_samples*sizeof(double));
|
||||
|
||||
ad->est_bias_p = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->est_bias_q = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->est_bias_r = malloc(ad->nb_samples*sizeof(double));
|
||||
|
||||
ad->P11 = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->P22 = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->P33 = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->P44 = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->P55 = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->P66 = malloc(ad->nb_samples*sizeof(double));
|
||||
ad->P77 = malloc(ad->nb_samples*sizeof(double));
|
||||
|
||||
return ad;
|
||||
}
|
||||
|
||||
|
||||
struct ahrs_data* ahrs_data_read_log(const char* filename) {
|
||||
GError* _err = NULL;
|
||||
GIOChannel* in_c = g_io_channel_new_file(filename, "r", &_err);
|
||||
if (_err) {
|
||||
g_free(_err);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
GArray* ga_gx = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
GArray* ga_gy = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
GArray* ga_gz = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
GArray* ga_ax = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
GArray* ga_ay = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
GArray* ga_az = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
GArray* ga_mx = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
GArray* ga_my = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
GArray* ga_mz = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
|
||||
GString *str = g_string_sized_new(256);
|
||||
GIOStatus st;
|
||||
do {
|
||||
double x, y, z;
|
||||
st = g_io_channel_read_line_string(in_c, str, NULL, &_err);
|
||||
if (g_str_has_prefix(str->str, "IMU_GYRO")) {
|
||||
if (sscanf(str->str, "IMU_GYRO %lf %lf %lf", &x, &y, &z) == 3) {
|
||||
g_array_append_val(ga_gx, x);
|
||||
g_array_append_val(ga_gy, y);
|
||||
g_array_append_val(ga_gz, z);
|
||||
}
|
||||
}
|
||||
else if (g_str_has_prefix(str->str, "IMU_ACCEL")) {
|
||||
if (sscanf(str->str, "IMU_ACCEL %lf %lf %lf", &x, &y, &z) == 3) {
|
||||
g_array_append_val(ga_ax, x);
|
||||
g_array_append_val(ga_ay, y);
|
||||
g_array_append_val(ga_az, z);
|
||||
}
|
||||
}
|
||||
else if (g_str_has_prefix(str->str, "IMU_MAG")) {
|
||||
if (sscanf(str->str, "IMU_MAG %lf %lf %lf", &x, &y, &z) == 3) {
|
||||
g_array_append_val(ga_mx, x);
|
||||
g_array_append_val(ga_my, y);
|
||||
g_array_append_val(ga_mz, z);
|
||||
}
|
||||
}
|
||||
}
|
||||
while (st != G_IO_STATUS_EOF);
|
||||
g_string_free(str, TRUE);
|
||||
g_free(in_c);
|
||||
|
||||
struct ahrs_data* ad = ahrs_data_new(ga_gx->len, 0.015625);
|
||||
int i;
|
||||
for (i=0; i<ga_gx->len; i++) {
|
||||
ad->t[i] = (double)i * ad->dt;
|
||||
|
||||
double* ggx = &g_array_index(ga_gx, double, i);
|
||||
ad->gyro_p[i] = *ggx;
|
||||
double* ggy = &g_array_index(ga_gy, double, i);
|
||||
ad->gyro_q[i] = *ggy;
|
||||
double* ggz = &g_array_index(ga_gz, double, i);
|
||||
ad->gyro_r[i] = *ggz;
|
||||
|
||||
double* gax = &g_array_index(ga_ax, double, i);
|
||||
ad->accel_x[i] = *gax;
|
||||
double* gay = &g_array_index(ga_ay, double, i);
|
||||
ad->accel_y[i] = *gay;
|
||||
double* gaz = &g_array_index(ga_az, double, i);
|
||||
ad->accel_z[i] = *gaz;
|
||||
|
||||
double* gmx = &g_array_index(ga_mx, double, i);
|
||||
ad->mag_x[i] = *gmx;
|
||||
double* gmy = &g_array_index(ga_my, double, i);
|
||||
ad->mag_y[i] = *gmy;
|
||||
double* gmz = &g_array_index(ga_mz, double, i);
|
||||
ad->mag_z[i] = *gmz;
|
||||
}
|
||||
|
||||
return ad;
|
||||
}
|
||||
|
||||
void ahrs_data_save_state_euler(struct ahrs_data* ad, int idx, double* X, double* P) {
|
||||
ad->est_phi[idx] = X[0];
|
||||
ad->est_theta[idx] = X[1];
|
||||
ad->est_psi[idx] = X[2];
|
||||
|
||||
ad->est_bias_p[idx] = X[3];
|
||||
ad->est_bias_q[idx] = X[4];
|
||||
ad->est_bias_r[idx] = X[5];
|
||||
|
||||
ad->P11[idx] = P[0];
|
||||
ad->P22[idx] = P[1*7 + 1];
|
||||
ad->P33[idx] = P[2*7 + 2];
|
||||
ad->P44[idx] = P[3*7 + 3];
|
||||
ad->P55[idx] = P[4*7 + 4];
|
||||
ad->P66[idx] = P[5*7 + 5];
|
||||
|
||||
}
|
||||
|
||||
void ahrs_data_save_state_quat(struct ahrs_data* ad, int idx, double* X, double* P) {
|
||||
double eulers[3];
|
||||
eulers_of_quat(eulers, X);
|
||||
ad->est_phi[idx] = eulers[0];
|
||||
ad->est_theta[idx] = eulers[1];
|
||||
ad->est_psi[idx] = eulers[2];
|
||||
|
||||
ad->est_bias_p[idx] = X[4];
|
||||
ad->est_bias_q[idx] = X[5];
|
||||
ad->est_bias_r[idx] = X[6];
|
||||
|
||||
ad->P11[idx] = P[0];
|
||||
ad->P22[idx] = P[1*7 + 1];
|
||||
ad->P33[idx] = P[2*7 + 2];
|
||||
ad->P44[idx] = P[3*7 + 3];
|
||||
ad->P55[idx] = P[4*7 + 4];
|
||||
ad->P66[idx] = P[5*7 + 5];
|
||||
ad->P77[idx] = P[6*7 + 6];
|
||||
|
||||
}
|
||||
|
||||
void ahrs_data_save_measure(struct ahrs_data* ad, int idx) {
|
||||
|
||||
ad->m_phi[idx] = phi_of_accel(ad->accel_x[idx], ad->accel_y[idx], ad->accel_z[idx]);
|
||||
ad->m_theta[idx] = theta_of_accel(ad->accel_x[idx], ad->accel_y[idx], ad->accel_z[idx]);
|
||||
ad->m_psi[idx] = psi_of_mag(ad->est_phi[idx], ad->est_theta[idx],
|
||||
ad->mag_x[idx], ad->mag_y[idx], ad->mag_z[idx]);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
#ifndef AHRS_DATA_H
|
||||
#define AHRS_DATA_H
|
||||
|
||||
struct ahrs_data {
|
||||
double dt;
|
||||
int nb_samples;
|
||||
double* t;
|
||||
|
||||
/* true state */
|
||||
double* phi;
|
||||
double* theta;
|
||||
double* psi;
|
||||
|
||||
double* bias_p;
|
||||
double* bias_q;
|
||||
double* bias_r;
|
||||
|
||||
/* sensors */
|
||||
double* gyro_p;
|
||||
double* gyro_q;
|
||||
double* gyro_r;
|
||||
|
||||
double* accel_x;
|
||||
double* accel_y;
|
||||
double* accel_z;
|
||||
|
||||
double* mag_x;
|
||||
double* mag_y;
|
||||
double* mag_z;
|
||||
|
||||
/* measures */
|
||||
double* m_phi;
|
||||
double* m_theta;
|
||||
double* m_psi;
|
||||
|
||||
/* estimated state */
|
||||
double* est_phi;
|
||||
double* est_theta;
|
||||
double* est_psi;
|
||||
|
||||
double* est_bias_p;
|
||||
double* est_bias_q;
|
||||
double* est_bias_r;
|
||||
|
||||
double* P11;
|
||||
double* P22;
|
||||
double* P33;
|
||||
double* P44;
|
||||
double* P55;
|
||||
double* P66;
|
||||
double* P77;
|
||||
};
|
||||
|
||||
extern struct ahrs_data* ahrs_data_new(int len, double dt);
|
||||
extern struct ahrs_data* ahrs_data_read_log(const char* filename);
|
||||
extern void ahrs_data_save_state_euler(struct ahrs_data* ad, int idx, double* X, double* P);
|
||||
extern void ahrs_data_save_state_quat(struct ahrs_data* ad, int idx, double* X, double* P);
|
||||
extern void ahrs_data_save_measure(struct ahrs_data* ad, int idx);
|
||||
extern void ahrs_data_save_state(struct ahrs_data* ad, int idx, double* X, double* P);
|
||||
|
||||
#endif /* AHRS_DATA_H */
|
||||
@@ -0,0 +1,160 @@
|
||||
#include "ahrs_display.h"
|
||||
|
||||
#include <gtkdatabox.h>
|
||||
#include <gtkdatabox_points.h>
|
||||
#include <gtkdatabox_lines.h>
|
||||
#include <gtkdatabox_bars.h>
|
||||
#include <gtkdatabox_grid.h>
|
||||
#include <gtkdatabox_cross_simple.h>
|
||||
#include <gtkdatabox_marker.h>
|
||||
|
||||
GtkWidget* ahrs_display(struct ahrs_data* ad) {
|
||||
GtkWidget* window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
|
||||
gtk_widget_set_size_request (window, 1280, 800);
|
||||
|
||||
GtkWidget* vbox1 = gtk_vbox_new (FALSE, 0);
|
||||
gtk_container_add (GTK_CONTAINER (window), vbox1);
|
||||
|
||||
GdkColor red = {0, 65535, 0, 0};
|
||||
GdkColor light_red = {0, 65535, 31875, 31875};
|
||||
GdkColor green = {0, 0, 65535, 0};
|
||||
GdkColor light_green = {0, 31875, 65535, 31875};
|
||||
GdkColor blue = {0, 0, 0, 65535};
|
||||
GdkColor light_blue = {0, 14025, 39525, 65535};
|
||||
// GdkColor pink = {0, 65535, 0, 65535};
|
||||
// GdkColor black = {0, 0, 0, 0};
|
||||
|
||||
GtkWidget *frame = gtk_frame_new ("rate");
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
|
||||
GtkWidget* databox = gtk_databox_new();
|
||||
gfloat* ft = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_mrate_p = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_mrate_q = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_mrate_r = g_new0 (gfloat, ad->nb_samples);
|
||||
int idx;
|
||||
for (idx=0; idx<ad->nb_samples; idx++) {
|
||||
ft[idx] = ad->t[idx];
|
||||
f_mrate_p[idx] = ad->gyro_p[idx];
|
||||
f_mrate_q[idx] = ad->gyro_q[idx];
|
||||
f_mrate_r[idx] = ad->gyro_r[idx];
|
||||
}
|
||||
GtkDataboxGraph *g_mrate_p = gtk_databox_lines_new (ad->nb_samples, ft, f_mrate_p, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_mrate_p);
|
||||
GtkDataboxGraph *g_mrate_q = gtk_databox_lines_new (ad->nb_samples, ft, f_mrate_q, &green, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_mrate_q);
|
||||
GtkDataboxGraph *g_mrate_r = gtk_databox_lines_new (ad->nb_samples, ft, f_mrate_r, &blue, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_mrate_r);
|
||||
|
||||
gtk_container_add (GTK_CONTAINER (frame), databox );
|
||||
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
|
||||
|
||||
|
||||
|
||||
frame = gtk_frame_new ("angle");
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
|
||||
databox = gtk_databox_new();
|
||||
// databox = gtk_databox_create_box_with_scrollbars_and_rulers();
|
||||
gfloat* f_ephi = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_etheta = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_epsi = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_mphi = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_mtheta = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_mpsi = g_new0 (gfloat, ad->nb_samples);
|
||||
for (idx=0; idx<ad->nb_samples; idx++) {
|
||||
f_ephi[idx] = ad->est_phi[idx];
|
||||
f_etheta[idx] = ad->est_theta[idx];
|
||||
f_epsi[idx] = ad->est_psi[idx];
|
||||
f_mphi[idx] = ad->m_phi[idx];
|
||||
f_mtheta[idx] = ad->m_theta[idx];
|
||||
f_mpsi[idx] = ad->m_psi[idx];
|
||||
}
|
||||
GtkDataboxGraph *g_ephi = gtk_databox_lines_new (ad->nb_samples, ft, f_ephi, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_ephi);
|
||||
GtkDataboxGraph *g_etheta = gtk_databox_lines_new (ad->nb_samples, ft, f_etheta, &green, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_etheta);
|
||||
GtkDataboxGraph *g_epsi = gtk_databox_lines_new (ad->nb_samples, ft, f_epsi, &blue, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_epsi);
|
||||
GtkDataboxGraph *g_mphi = gtk_databox_lines_new (ad->nb_samples, ft, f_mphi, &light_red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_mphi);
|
||||
GtkDataboxGraph *g_mtheta = gtk_databox_lines_new (ad->nb_samples, ft, f_mtheta, &light_green, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_mtheta);
|
||||
GtkDataboxGraph *g_mpsi = gtk_databox_lines_new (ad->nb_samples, ft, f_mpsi, &light_blue, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_mpsi);
|
||||
|
||||
gtk_container_add (GTK_CONTAINER (frame), databox );
|
||||
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
|
||||
|
||||
|
||||
|
||||
frame = gtk_frame_new ("biase");
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
|
||||
databox = gtk_databox_new();
|
||||
gfloat* f_ebias_p = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_ebias_q = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_ebias_r = g_new0 (gfloat, ad->nb_samples);
|
||||
for (idx=0; idx<ad->nb_samples; idx++) {
|
||||
f_ebias_p[idx] = ad->est_bias_p[idx];
|
||||
f_ebias_q[idx] = ad->est_bias_q[idx];
|
||||
f_ebias_r[idx] = ad->est_bias_r[idx];
|
||||
}
|
||||
GtkDataboxGraph *ge_bias_p = gtk_databox_lines_new (ad->nb_samples, ft, f_ebias_p, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), ge_bias_p);
|
||||
GtkDataboxGraph *ge_bias_q = gtk_databox_lines_new (ad->nb_samples, ft, f_ebias_q, &green, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), ge_bias_q);
|
||||
GtkDataboxGraph *ge_bias_r = gtk_databox_lines_new (ad->nb_samples, ft, f_ebias_r, &blue, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), ge_bias_r);
|
||||
|
||||
gtk_container_add (GTK_CONTAINER (frame), databox );
|
||||
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
|
||||
|
||||
|
||||
|
||||
frame = gtk_frame_new ("covariance");
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
|
||||
databox = gtk_databox_new();
|
||||
gfloat* f_p11 = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_p22 = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_p33 = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_p44 = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_p55 = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_p66 = g_new0 (gfloat, ad->nb_samples);
|
||||
gfloat* f_p77 = g_new0 (gfloat, ad->nb_samples);
|
||||
for (idx=0; idx<ad->nb_samples; idx++) {
|
||||
f_p11[idx] = ad->P11[idx];
|
||||
f_p22[idx] = ad->P22[idx];
|
||||
f_p33[idx] = ad->P33[idx];
|
||||
f_p44[idx] = ad->P44[idx];
|
||||
f_p55[idx] = ad->P55[idx];
|
||||
f_p66[idx] = ad->P66[idx];
|
||||
f_p77[idx] = ad->P77[idx];
|
||||
}
|
||||
GtkDataboxGraph *g_p11 = gtk_databox_lines_new (ad->nb_samples, ft, f_p11, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_p11);
|
||||
GtkDataboxGraph *g_p22 = gtk_databox_lines_new (ad->nb_samples, ft, f_p22, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_p22);
|
||||
GtkDataboxGraph *g_p33 = gtk_databox_lines_new (ad->nb_samples, ft, f_p33, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_p33);
|
||||
GtkDataboxGraph *g_p44 = gtk_databox_lines_new (ad->nb_samples, ft, f_p44, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_p44);
|
||||
GtkDataboxGraph *g_p55 = gtk_databox_lines_new (ad->nb_samples, ft, f_p55, &blue, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_p55);
|
||||
GtkDataboxGraph *g_p66 = gtk_databox_lines_new (ad->nb_samples, ft, f_p66, &blue, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_p66);
|
||||
GtkDataboxGraph *g_p77 = gtk_databox_lines_new (ad->nb_samples, ft, f_p77, &blue, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_p77);
|
||||
|
||||
gtk_container_add (GTK_CONTAINER (frame), databox );
|
||||
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
|
||||
|
||||
|
||||
gtk_widget_show_all(window);
|
||||
return window;
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
#ifndef AHRS_DISPLAY_H
|
||||
#define AHRS_DISPLAY_H
|
||||
|
||||
#include <gtk/gtk.h>
|
||||
|
||||
#include "ahrs_data.h"
|
||||
|
||||
extern GtkWidget* ahrs_display(struct ahrs_data* td);
|
||||
|
||||
#endif /* AHRS_DISPLAY_H */
|
||||
@@ -0,0 +1,192 @@
|
||||
#include "ahrs_data.h"
|
||||
#include "ahrs_display.h"
|
||||
#include "ahrs_utils.h"
|
||||
#include "ekf.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
static struct ahrs_data* ad;
|
||||
#define UPDATE_PHI 0
|
||||
#define UPDATE_THETA 1
|
||||
#define UPDATE_PSI 2
|
||||
#define UPDATE_NB 3
|
||||
static int ahrs_state;
|
||||
|
||||
|
||||
void linear_filter(double *u, double* X, double* dt, double *Xdot, double* F) {
|
||||
*dt = ad->dt;
|
||||
|
||||
double p = u[0] - X[3];
|
||||
double q = u[1] - X[4];
|
||||
double r = u[2] - X[5];
|
||||
|
||||
double phi = X[0];
|
||||
double theta = X[1];
|
||||
// double psi = X[2];
|
||||
|
||||
Xdot[0] = p + sin(phi)*tan(theta) * q + cos(phi)*tan(theta) * r;
|
||||
Xdot[1] = cos(phi) * q - sin(phi) * r;
|
||||
Xdot[2] = sin(phi)/cos(theta) * q + cos(phi)/cos(theta) * r;
|
||||
Xdot[3] = 0.;
|
||||
Xdot[4] = 0.;
|
||||
Xdot[5] = 0.;
|
||||
|
||||
F[0*6+0] = cos(phi)*tan(theta)*q - sin(phi)*tan(theta)*r;
|
||||
F[0*6+1] = 1/(1+theta*theta) * (sin(phi)*q+cos(phi)*r);
|
||||
F[0*6+2] = 0.;
|
||||
F[0*6+3] = -1.;
|
||||
F[0*6+4] = -sin(phi)*tan(theta);
|
||||
F[0*6+5] = -cos(phi)*tan(theta);
|
||||
|
||||
F[1*6+0] = -sin(phi)*q - cos(phi)*r;
|
||||
F[1*6+1] = 0.;
|
||||
F[1*6+2] = 0.;
|
||||
F[1*6+3] = 0.;
|
||||
F[1*6+4] = -cos(phi);
|
||||
F[1*6+5] = sin(phi);
|
||||
|
||||
F[2*6+0] = cos(phi)/cos(theta)*q - sin(phi)/cos(theta)*r;
|
||||
F[2*6+1] = sin(theta)/(cos(theta)*cos(theta))*(sin(phi)*q+cos(phi)*r);
|
||||
F[2*6+2] = 0.;
|
||||
F[2*6+3] = 0.;
|
||||
F[2*6+4] = -sin(phi)/cos(theta);
|
||||
F[2*6+5] = -cos(phi)/cos(theta);
|
||||
|
||||
F[3*6+0] = 0.;
|
||||
F[3*6+1] = 0.;
|
||||
F[3*6+2] = 0.;
|
||||
F[3*6+3] = 0.;
|
||||
F[3*6+4] = 0.;
|
||||
F[3*6+5] = 0.;
|
||||
|
||||
F[4*6+0] = 0.;
|
||||
F[4*6+1] = 0.;
|
||||
F[4*6+2] = 0.;
|
||||
F[4*6+3] = 0.;
|
||||
F[4*6+4] = 0.;
|
||||
F[4*6+5] = 0.;
|
||||
|
||||
F[5*6+0] = 0.;
|
||||
F[5*6+1] = 0.;
|
||||
F[5*6+2] = 0.;
|
||||
F[5*6+3] = 0.;
|
||||
F[5*6+4] = 0.;
|
||||
F[5*6+5] = 0.;
|
||||
|
||||
}
|
||||
|
||||
void linear_measure(double *y, double* err, double*X, double *H) {
|
||||
*err = y[0] - X[ahrs_state];
|
||||
switch (ahrs_state) {
|
||||
case UPDATE_PHI: {
|
||||
WARP(*err, M_PI);
|
||||
H[0] = 1.;
|
||||
H[1] = 0.;
|
||||
H[2] = 0.;
|
||||
break;
|
||||
}
|
||||
case UPDATE_THETA: {
|
||||
WARP(*err, M_PI_2);
|
||||
H[0] = 0.;
|
||||
H[1] = 1.;
|
||||
H[2] = 0.;
|
||||
break;
|
||||
}
|
||||
case UPDATE_PSI: {
|
||||
WARP(*err, M_PI);
|
||||
H[0] = 0.;
|
||||
H[1] = 0.;
|
||||
H[2] = 1.;
|
||||
break;
|
||||
}
|
||||
}
|
||||
H[3] = 0.;
|
||||
H[4] = 0.;
|
||||
H[5] = 0.;
|
||||
}
|
||||
|
||||
|
||||
#define P0E 1.0
|
||||
#define P0B 1.0
|
||||
|
||||
#define QE 0.001
|
||||
#define QB 0.008
|
||||
|
||||
void run_ekf (void) {
|
||||
/* initial state covariance matrix */
|
||||
double P[6*6] = {P0E, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, P0E, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, P0E, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, P0B, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, P0B, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, P0B };
|
||||
/* model noise covariance matrix */
|
||||
double Q[6*6] = { QE, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, QE, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, QE, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, QB, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, QB, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, QB };
|
||||
/* measurement noise covariance matrix */
|
||||
double R[1]={1.7};
|
||||
/* state [phi, theta, psi, bp, bq, br] */
|
||||
double X[6];
|
||||
/* measure */
|
||||
double Y[1];
|
||||
/* command */
|
||||
double U[3] = {0.0, 0.0, 0.0};
|
||||
|
||||
struct ekf_filter* ekf;
|
||||
ekf = ekf_filter_new(6, 1, Q, R, linear_filter, linear_measure);
|
||||
ahrs_euler_init(ad, 150, X);
|
||||
ekf_filter_reset(ekf, X, P);
|
||||
int iter;
|
||||
|
||||
for (iter=0; iter < ad->nb_samples; iter++) {
|
||||
U[0] = ad->gyro_p[iter];
|
||||
U[1] = ad->gyro_q[iter];
|
||||
U[2] = ad->gyro_r[iter];
|
||||
ekf_filter_predict(ekf, U);
|
||||
|
||||
ahrs_state = iter%3;
|
||||
switch (ahrs_state) {
|
||||
case UPDATE_PHI:
|
||||
Y[0] = phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
|
||||
break;
|
||||
case UPDATE_THETA:
|
||||
Y[0] = theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
|
||||
break;
|
||||
case UPDATE_PSI: {
|
||||
double m_phi = phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
|
||||
double m_theta = theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
|
||||
Y[0] = psi_of_mag(m_phi, m_theta, ad->mag_x[iter], ad->mag_y[iter], ad->mag_z[iter]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
ekf_filter_update(ekf, Y);
|
||||
|
||||
ekf_filter_get_state(ekf, X, P);
|
||||
ahrs_data_save_state_euler(ad, iter, X, P);
|
||||
ahrs_data_save_measure(ad, iter);
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
int
|
||||
main(int argc, char *argv[]) {
|
||||
gtk_init(&argc, &argv);
|
||||
|
||||
ad = ahrs_data_read_log("../data/log_ahrs_bug");
|
||||
//ad = ahrs_data_read_log("../data/log_ahrs_roll");
|
||||
//ad = ahrs_data_read_log("../data/log_ahrs_yaw_pitched");
|
||||
run_ekf();
|
||||
ahrs_display(ad);
|
||||
gtk_main();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,197 @@
|
||||
#include "ahrs_data.h"
|
||||
#include "ahrs_display.h"
|
||||
#include "ahrs_utils.h"
|
||||
#include "ekf.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
static struct ahrs_data* ad;
|
||||
#define UPDATE_PHI 0
|
||||
#define UPDATE_THETA 1
|
||||
#define UPDATE_PSI 2
|
||||
#define UPDATE_NB 3
|
||||
static int ahrs_state;
|
||||
|
||||
void linear_filter(double *u, double* X, double* dt, double *Xdot, double* F) {
|
||||
*dt = ad->dt;
|
||||
|
||||
/*
|
||||
* quat_dot = Wxq(pqr) * quat
|
||||
* bias_dot = 0
|
||||
*
|
||||
* Wxq is the quaternion omega matrix:
|
||||
*
|
||||
* [ 0, -p, -q, -r ]
|
||||
* 1/2 * [ p, 0, r, -q ]
|
||||
* [ q, -r, 0, p ]
|
||||
* [ r, q, -p, 0 ]
|
||||
*/
|
||||
double p = u[0] - X[4];
|
||||
double q = u[1] - X[5];
|
||||
double r = u[2] - X[6];
|
||||
double q0 = X[0];
|
||||
double q1 = X[1];
|
||||
double q2 = X[2];
|
||||
double q3 = X[3];
|
||||
|
||||
double q0_dot = -p*q1 -q*q2 -r*q3;
|
||||
double q1_dot = p*q0 +r*q2 -q*q3;
|
||||
double q2_dot = q*q0 -r*q1 +p*q3;
|
||||
double q3_dot = r*q0 +q*q1 -p*q2 ;
|
||||
|
||||
Xdot[0] = q0_dot;
|
||||
Xdot[1] = q1_dot;
|
||||
Xdot[2] = q2_dot;
|
||||
Xdot[3] = q3_dot;
|
||||
Xdot[4] = 0.;
|
||||
Xdot[5] = 0.;
|
||||
Xdot[6] = 0.;
|
||||
|
||||
/*
|
||||
* F is the Jacobian of Xdot wrt the state
|
||||
*
|
||||
*
|
||||
*
|
||||
*/
|
||||
double my_f[] = { 0., -p, -q, -r, q1, q2, q3,
|
||||
p , 0., r, -q, -q0, q3, -q2,
|
||||
q, -r, 0., p, -q3, -q0, q1,
|
||||
r, q, -p, 0., q2, -q1, -q0,
|
||||
0., 0., 0., 0., 0., 0., 0.,
|
||||
0., 0., 0., 0., 0., 0., 0.,
|
||||
0., 0., 0., 0., 0., 0., 0. };
|
||||
memcpy(F, my_f, sizeof(my_f));
|
||||
|
||||
}
|
||||
|
||||
void linear_measure(double *y, double* err, double*X, double *H) {
|
||||
double eulers[3];
|
||||
eulers_of_quat(eulers, X);
|
||||
*err = y[0] - eulers[ahrs_state];
|
||||
/* H is the jacobian of the measure wrt X */
|
||||
double q0 = X[0];
|
||||
double q1 = X[1];
|
||||
double q2 = X[2];
|
||||
double q3 = X[3];
|
||||
double dcm00 = 1.0-2*(q2*q2 + q3*q3);
|
||||
double dcm01 = 2*(q1*q2 + q0*q3);
|
||||
double dcm02 = 2*(q1*q3 - q0*q2);
|
||||
double dcm12 = 2*(q2*q3 + q0*q1);
|
||||
double dcm22 = 1.0-2*(q1*q1 + q2*q2);
|
||||
switch (ahrs_state) {
|
||||
case UPDATE_PHI: {
|
||||
WARP(*err, M_PI);
|
||||
const double phi_err = 2. / (dcm22*dcm22 + dcm12*dcm12);
|
||||
H[0] = (q1 * dcm22) * phi_err;
|
||||
H[1] = (q0 * dcm22 + 2 * q1 * dcm12) * phi_err;
|
||||
H[2] = (q3 * dcm22 + 2 * q2 * dcm12) * phi_err;
|
||||
H[3] = (q2 * dcm22) * phi_err;
|
||||
break;
|
||||
}
|
||||
case UPDATE_THETA: {
|
||||
WARP(*err, M_PI_2);
|
||||
const double theta_err = -2. / sqrt( 1 - dcm02*dcm02 );
|
||||
H[0] = -q2 * theta_err;
|
||||
H[1] = q3 * theta_err;
|
||||
H[2] = -q0 * theta_err;
|
||||
H[3] = q1 * theta_err;
|
||||
break;
|
||||
}
|
||||
case UPDATE_PSI: {
|
||||
WARP(*err, M_PI);
|
||||
const double psi_err = 2. / (dcm00*dcm00 + dcm01*dcm01);
|
||||
H[0] = (q3 * dcm00) * psi_err;
|
||||
H[1] = (q2 * dcm00) * psi_err;
|
||||
H[2] = (q1 * dcm00 + 2 * q2 * dcm01) * psi_err;
|
||||
H[3] = (q0 * dcm00 + 2 * q3 * dcm01) * psi_err;
|
||||
break;
|
||||
}
|
||||
}
|
||||
H[4] = 0.;
|
||||
H[5] = 0.;
|
||||
H[6] = 0.;
|
||||
}
|
||||
|
||||
#define P0Q 1.0
|
||||
#define P0B 1.0
|
||||
|
||||
#define Q0G 0.001
|
||||
#define Q0B 0.008
|
||||
|
||||
void run_ekf (void) {
|
||||
/* initial state covariance matrix */
|
||||
double P[7*7] = {P0Q, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, P0Q, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, P0Q, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, P0Q, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, P0B, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, P0B, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, P0B };
|
||||
/* model noise covariance matrix */
|
||||
double Q[7*7] = {Q0G, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, Q0G, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, Q0G, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, Q0G, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, Q0B, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, Q0B, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Q0B };
|
||||
/* measurement noise covariance matrix */
|
||||
double R[1]={1.7};
|
||||
/* state [q0, q1, q2, q3, bp, bq, br] */
|
||||
double X[7];
|
||||
/* measure */
|
||||
double Y[1];
|
||||
/* command */
|
||||
double U[3] = {0.0, 0.0, 0.0};
|
||||
|
||||
struct ekf_filter* ekf;
|
||||
ekf = ekf_filter_new(7, 1, Q, R, linear_filter, linear_measure);
|
||||
ahrs_quat_init(ad, 150, X);
|
||||
ekf_filter_reset(ekf, X, P);
|
||||
|
||||
/* filter run */
|
||||
int iter;
|
||||
for (iter=0; iter < ad->nb_samples; iter++) {
|
||||
U[0] = ad->gyro_p[iter];// - X[4];
|
||||
U[1] = ad->gyro_q[iter];// - X[5];
|
||||
U[2] = ad->gyro_r[iter];// - X[6];
|
||||
ekf_filter_predict(ekf, U);
|
||||
norm_quat(X);
|
||||
ahrs_state = iter%3;
|
||||
switch (ahrs_state) {
|
||||
case UPDATE_PHI:
|
||||
Y[0] = phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
|
||||
break;
|
||||
case UPDATE_THETA:
|
||||
Y[0] = theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
|
||||
break;
|
||||
case UPDATE_PSI: {
|
||||
double eulers[3];
|
||||
eulers_of_quat(eulers, X);
|
||||
Y[0] = psi_of_mag(eulers[0], eulers[1], ad->mag_x[iter], ad->mag_y[iter], ad->mag_z[iter]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
ekf_filter_update(ekf, Y);
|
||||
norm_quat(X);
|
||||
// printf("P66 %f\n", P[6*7 + 6]);
|
||||
ekf_filter_get_state(ekf, X, P);
|
||||
ahrs_data_save_state_quat(ad, iter, X, P);
|
||||
ahrs_data_save_measure(ad, iter);
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
main(int argc, char *argv[]) {
|
||||
gtk_init(&argc, &argv);
|
||||
|
||||
ad = ahrs_data_read_log("../data/log_ahrs_bug");
|
||||
//ad = ahrs_data_read_log("../data/log_ahrs_roll");
|
||||
//ad = ahrs_data_read_log("../data/log_ahrs_yaw_pitched");
|
||||
run_ekf();
|
||||
ahrs_display(ad);
|
||||
gtk_main();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,404 @@
|
||||
#include "ahrs_quat_fast_ekf.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <inttypes.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
/* Time step */
|
||||
#define afe_dt 0.0166667
|
||||
|
||||
/* We have seven variables in our state -- the quaternion attitude
|
||||
* estimate and three gyro bias values
|
||||
*/
|
||||
FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3;
|
||||
FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r;
|
||||
|
||||
/* We maintain unbiased rates. */
|
||||
FLOAT_T afe_p, afe_q, afe_r;
|
||||
|
||||
/* We maintain eulers */
|
||||
FLOAT_T afe_phi, afe_theta, afe_psi;
|
||||
|
||||
/* time derivative of state
|
||||
* we know that bias_p_dot = bias_q_dot = bias_r_dot = 0
|
||||
*/
|
||||
FLOAT_T afe_q0_dot, afe_q1_dot, afe_q2_dot, afe_q3_dot;
|
||||
|
||||
/*
|
||||
* The Direction Cosine Matrix is used to help rotate measurements
|
||||
* to and from the body frame. We only need five elements from it,
|
||||
* so those are computed explicitly rather than the entire matrix
|
||||
*
|
||||
* External routines might want these (to until sensor readings),
|
||||
* so we export them.
|
||||
*/
|
||||
FLOAT_T afe_dcm00;
|
||||
FLOAT_T afe_dcm01;
|
||||
FLOAT_T afe_dcm02;
|
||||
FLOAT_T afe_dcm12;
|
||||
FLOAT_T afe_dcm22;
|
||||
|
||||
/*
|
||||
* Covariance matrix and covariance matrix derivative
|
||||
*/
|
||||
FLOAT_T afe_P[7][7];
|
||||
FLOAT_T afe_Pdot[7][7];
|
||||
|
||||
/*
|
||||
* F represents the Jacobian of the derivative of the system with respect
|
||||
* its states. We do not allocate the bottom three rows since we know that
|
||||
* the derivatives of bias_dot are all zero.
|
||||
*/
|
||||
FLOAT_T afe_F[4][7];
|
||||
|
||||
/*
|
||||
* Kalman filter variables.
|
||||
*/
|
||||
FLOAT_T afe_PHt[7];
|
||||
FLOAT_T afe_K[7];
|
||||
FLOAT_T afe_E;
|
||||
|
||||
/*
|
||||
* H represents the Jacobian of the measurements of the attitude
|
||||
* with respect to the states of the filter. We do not allocate the bottom
|
||||
* three rows since we know that the attitude measurements have no
|
||||
* relationship to gyro bias.
|
||||
*/
|
||||
FLOAT_T afe_H[4];
|
||||
/* temporary variable used during state covariance update */
|
||||
FLOAT_T afe_FP[4][7];
|
||||
|
||||
/*
|
||||
* Q is our estimate noise variance. It is supposed to be an NxN
|
||||
* matrix, but with elements only on the diagonals. Additionally,
|
||||
* since the quaternion has no expected noise (we can't directly measure
|
||||
* it), those are zero. For the gyro, we expect around 5 deg/sec noise,
|
||||
* which is 0.08 rad/sec. The variance is then 0.08^2 ~= 0.0075.
|
||||
*/
|
||||
/* I measured about 0.009 rad/s noise */
|
||||
#define afe_Q_gyro 8e-03
|
||||
|
||||
/*
|
||||
* R is our measurement noise estimate. Like Q, it is supposed to be
|
||||
* an NxN matrix with elements on the diagonals. However, since we can
|
||||
* not directly measure the gyro bias, we have no estimate for it.
|
||||
* We only have an expected noise in the pitch and roll accelerometers
|
||||
* and in the compass.
|
||||
*/
|
||||
#define AFE_R_PHI 1.3 * 1.3
|
||||
#define AFE_R_THETA 1.3 * 1.3
|
||||
#define AFE_R_PSI 2.5 * 2.5
|
||||
|
||||
#include "ahrs_quat_fast_ekf_utils.h"
|
||||
|
||||
/*
|
||||
* Call ahrs_state_update every dt seconds with the raw body frame angular
|
||||
* rates. It updates the attitude state estimate via this function:
|
||||
*
|
||||
* quat_dot = Wxq(pqr) * quat
|
||||
* bias_dot = 0
|
||||
*
|
||||
* Since F also contains Wxq, we fill it in here and then reuse the computed
|
||||
* values. This avoids the extra floating point math.
|
||||
*
|
||||
* Wxq is the quaternion omega matrix:
|
||||
*
|
||||
* [ 0, -p, -q, -r ]
|
||||
* 1/2 * [ p, 0, r, -q ]
|
||||
* [ q, -r, 0, p ]
|
||||
* [ r, q, -p, 0 ]
|
||||
*
|
||||
*
|
||||
*
|
||||
*
|
||||
* [ 0 -p -q -r q1 q2 q3]
|
||||
* F = 1/2 * [ p 0 r -q -q0 q3 -q2]
|
||||
* [ q -r 0 p -q3 q0 q1]
|
||||
* [ r q -p 0 q2 -q1 -q0]
|
||||
* [ 0 0 0 0 0 0 0]
|
||||
* [ 0 0 0 0 0 0 0]
|
||||
* [ 0 0 0 0 0 0 0]
|
||||
*
|
||||
*/
|
||||
void afe_predict( const FLOAT_T* gyro ) {
|
||||
/* Unbias our gyro values */
|
||||
afe_p = gyro[0] - afe_bias_p;
|
||||
afe_q = gyro[1] - afe_bias_q;
|
||||
afe_r = gyro[2] - afe_bias_r;
|
||||
|
||||
/* compute F
|
||||
F is only needed later on to update the state covariance P.
|
||||
However, its [0:3][0:3] region is actually the Wxq(pqr) which is needed to
|
||||
compute the time derivative of the quaternion, so we compute F now */
|
||||
|
||||
/* Fill in Wxq(pqr) into F */
|
||||
afe_F[0][0] = afe_F[1][1] = afe_F[2][2] = afe_F[3][3] = 0;
|
||||
afe_F[1][0] = afe_F[2][3] = afe_p * 0.5;
|
||||
afe_F[2][0] = afe_F[3][1] = afe_q * 0.5;
|
||||
afe_F[3][0] = afe_F[1][2] = afe_r * 0.5;
|
||||
|
||||
afe_F[0][1] = afe_F[3][2] = -afe_F[1][0];
|
||||
afe_F[0][2] = afe_F[1][3] = -afe_F[2][0];
|
||||
afe_F[0][3] = afe_F[2][1] = -afe_F[3][0];
|
||||
/* Fill in [4:6][0:3] region */
|
||||
afe_F[0][4] = afe_F[2][6] = afe_q1 * 0.5;
|
||||
afe_F[0][5] = afe_F[3][4] = afe_q2 * 0.5;
|
||||
afe_F[0][6] = afe_F[1][5] = afe_q3 * 0.5;
|
||||
|
||||
afe_F[1][4] = afe_F[2][5] = afe_F[3][6] = afe_q0 * -0.5;
|
||||
afe_F[3][5] = -afe_F[0][4];
|
||||
afe_F[1][6] = -afe_F[0][5];
|
||||
afe_F[2][4] = -afe_F[0][6];
|
||||
|
||||
/* update state */
|
||||
/* quat_dot = Wxq(pqr) * quat */
|
||||
afe_q0_dot = afe_F[0][1] * afe_q1 + afe_F[0][2] * afe_q2 + afe_F[0][3] * afe_q3;
|
||||
afe_q1_dot = afe_F[1][0] * afe_q0 + afe_F[1][2] * afe_q2 + afe_F[1][3] * afe_q3;
|
||||
afe_q2_dot = afe_F[2][0] * afe_q0 + afe_F[2][1] * afe_q1 + afe_F[2][3] * afe_q3;
|
||||
afe_q3_dot = afe_F[3][0] * afe_q0 + afe_F[3][1] * afe_q1 + afe_F[3][2] * afe_q2;
|
||||
|
||||
/* quat = quat + quat_dot * dt */
|
||||
afe_q0 += afe_q0_dot * afe_dt;
|
||||
afe_q1 += afe_q1_dot * afe_dt;
|
||||
afe_q2 += afe_q2_dot * afe_dt;
|
||||
afe_q3 += afe_q3_dot * afe_dt;
|
||||
|
||||
/* normalize quaternion */
|
||||
AFE_NORM_QUAT();
|
||||
/* */
|
||||
AFE_DCM_OF_QUAT();
|
||||
AFE_EULER_OF_DCM();
|
||||
|
||||
#ifdef EKF_UPDATE_DISCRETE
|
||||
/*
|
||||
* update covariance
|
||||
* Pdot = F*P*F' + Q
|
||||
* P += Pdot * dt
|
||||
*/
|
||||
uint8_t i, j, k;
|
||||
for (i=0; i<4; i++) {
|
||||
for (j=0; j<7; j++) {
|
||||
afe_FP[i][j] = 0.;
|
||||
for (k=0; k<7; k++) {
|
||||
afe_FP[i][j] += afe_F[i][k] * afe_P[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i=0; i<4; i++) {
|
||||
for (j=0; j<4; j++) {
|
||||
afe_Pdot[i][j] = 0.;
|
||||
for (k=0; k<7; k++) {
|
||||
afe_Pdot[i][j] += afe_FP[i][k] * afe_F[j][k];
|
||||
}
|
||||
}
|
||||
}
|
||||
/* add Q !!! added below*/
|
||||
// Pdot[4][4] = afe_Q_gyro;
|
||||
// Pdot[5][5] = afe_Q_gyro;
|
||||
// Pdot[6][6] = afe_Q_gyro;
|
||||
|
||||
/* P = P + Pdot * dt */
|
||||
for (i=0; i<4; i++)
|
||||
for (j=0; j<4; j++)
|
||||
afe_P[i][j] += afe_Pdot[i][j] * afe_dt;
|
||||
afe_P[4][4] += afe_Q_gyro * afe_dt;
|
||||
afe_P[5][5] += afe_Q_gyro * afe_dt;
|
||||
afe_P[6][6] += afe_Q_gyro * afe_dt;
|
||||
#endif
|
||||
|
||||
#ifdef EKF_UPDATE_CONTINUOUS
|
||||
/* Pdot = F*P + F'*P + Q */
|
||||
memset( afe_Pdot, 0, sizeof(afe_Pdot) );
|
||||
/*
|
||||
* Compute the A*P+P*At for the bottom rows of P and A_tranpose
|
||||
*/
|
||||
uint8_t i, j, k;
|
||||
for( i=0 ; i<4 ; i++ )
|
||||
for( j=0 ; j<7 ; j++ )
|
||||
for( k=4 ; k<7 ; k++ )
|
||||
{
|
||||
const FLOAT_T F_i_k = afe_F[i][k];
|
||||
afe_Pdot[i][j] += F_i_k * afe_P[k][j];
|
||||
afe_Pdot[j][i] += afe_P[j][k] * F_i_k;
|
||||
}
|
||||
/*
|
||||
* Compute F*P + P*Ft for the region [0..3][0..3]
|
||||
*/
|
||||
for( i=0 ; i<4 ; i++ )
|
||||
for( j=0 ; j<4 ; j++ )
|
||||
for( k=0 ; k<4 ; k++ )
|
||||
{
|
||||
/* The diagonals of A are zero */
|
||||
if( i == k && j == k )
|
||||
continue;
|
||||
if( j == k )
|
||||
afe_Pdot[i][j] += afe_F[i][k] * afe_P[k][j];
|
||||
else
|
||||
if( i == k )
|
||||
afe_Pdot[i][j] += afe_P[i][k] * afe_F[j][k];
|
||||
else
|
||||
afe_Pdot[i][j] += ( afe_F[i][k] * afe_P[k][j] +
|
||||
afe_P[i][k] * afe_F[j][k] );
|
||||
}
|
||||
/* Add in the non-zero parts of Q. The quaternion portions
|
||||
* are all zero, and all the gyros share the same value.
|
||||
*/
|
||||
for( i=4 ; i<7 ; i++ )
|
||||
afe_Pdot[i][i] += afe_Q_gyro;
|
||||
|
||||
/* Compute P = P + Pdot * dt */
|
||||
for( i=0 ; i<7 ; i++ )
|
||||
for( j=0 ; j<7 ; j++ )
|
||||
afe_P[i][j] += afe_Pdot[i][j] * afe_dt;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static void run_kalman( const FLOAT_T R_axis, const FLOAT_T error ) {
|
||||
int i, j;
|
||||
|
||||
/* PHt = P * H' */
|
||||
afe_PHt[0] = afe_P[0][0] * afe_H[0] + afe_P[0][1] * afe_H[1] + afe_P[0][2] * afe_H[2] + afe_P[0][3] * afe_H[3];
|
||||
afe_PHt[1] = afe_P[1][0] * afe_H[0] + afe_P[1][1] * afe_H[1] + afe_P[1][2] * afe_H[2] + afe_P[1][3] * afe_H[3];
|
||||
afe_PHt[2] = afe_P[2][0] * afe_H[0] + afe_P[2][1] * afe_H[1] + afe_P[2][2] * afe_H[2] + afe_P[2][3] * afe_H[3];
|
||||
afe_PHt[3] = afe_P[3][0] * afe_H[0] + afe_P[3][1] * afe_H[1] + afe_P[3][2] * afe_H[2] + afe_P[3][3] * afe_H[3];
|
||||
afe_PHt[4] = afe_P[4][0] * afe_H[0] + afe_P[4][1] * afe_H[1] + afe_P[4][2] * afe_H[2] + afe_P[4][3] * afe_H[3];
|
||||
afe_PHt[5] = afe_P[5][0] * afe_H[0] + afe_P[5][1] * afe_H[1] + afe_P[5][2] * afe_H[2] + afe_P[5][3] * afe_H[3];
|
||||
afe_PHt[6] = afe_P[6][0] * afe_H[0] + afe_P[6][1] * afe_H[1] + afe_P[6][2] * afe_H[2] + afe_P[6][3] * afe_H[3];
|
||||
|
||||
/* E = H * PHt + R */
|
||||
afe_E = R_axis;
|
||||
afe_E += afe_H[0] * afe_PHt[0];
|
||||
afe_E += afe_H[1] * afe_PHt[1];
|
||||
afe_E += afe_H[2] * afe_PHt[2];
|
||||
afe_E += afe_H[3] * afe_PHt[3];
|
||||
|
||||
/* Compute the inverse of E */
|
||||
afe_E = 1.0 / afe_E;
|
||||
|
||||
/* Compute K = P * H' * inv(E) */
|
||||
afe_K[0] = afe_PHt[0] * afe_E;
|
||||
afe_K[1] = afe_PHt[1] * afe_E;
|
||||
afe_K[2] = afe_PHt[2] * afe_E;
|
||||
afe_K[3] = afe_PHt[3] * afe_E;
|
||||
afe_K[4] = afe_PHt[4] * afe_E;
|
||||
afe_K[5] = afe_PHt[5] * afe_E;
|
||||
afe_K[6] = afe_PHt[6] * afe_E;
|
||||
|
||||
/* Update our covariance matrix: P = P - K * H * P */
|
||||
|
||||
/* Compute HP = H * P, reusing the PHt array. */
|
||||
afe_PHt[0] = afe_H[0] * afe_P[0][0] + afe_H[1] * afe_P[1][0] + afe_H[2] * afe_P[2][0] + afe_H[3] * afe_P[3][0];
|
||||
afe_PHt[1] = afe_H[0] * afe_P[0][1] + afe_H[1] * afe_P[1][1] + afe_H[2] * afe_P[2][1] + afe_H[3] * afe_P[3][1];
|
||||
afe_PHt[2] = afe_H[0] * afe_P[0][2] + afe_H[1] * afe_P[1][2] + afe_H[2] * afe_P[2][2] + afe_H[3] * afe_P[3][2];
|
||||
afe_PHt[3] = afe_H[0] * afe_P[0][3] + afe_H[1] * afe_P[1][3] + afe_H[2] * afe_P[2][3] + afe_H[3] * afe_P[3][3];
|
||||
afe_PHt[4] = afe_H[0] * afe_P[0][4] + afe_H[1] * afe_P[1][4] + afe_H[2] * afe_P[2][4] + afe_H[3] * afe_P[3][4];
|
||||
afe_PHt[5] = afe_H[0] * afe_P[0][5] + afe_H[1] * afe_P[1][5] + afe_H[2] * afe_P[2][5] + afe_H[3] * afe_P[3][5];
|
||||
afe_PHt[6] = afe_H[0] * afe_P[0][6] + afe_H[1] * afe_P[1][6] + afe_H[2] * afe_P[2][6] + afe_H[3] * afe_P[3][6];
|
||||
|
||||
/* Compute P -= K * HP (aliased to PHt) */
|
||||
for( i=0 ; i<4 ; i++ )
|
||||
for( j=0 ; j<7 ; j++ )
|
||||
afe_P[i][j] -= afe_K[i] * afe_PHt[j];
|
||||
|
||||
/* Update our state: X += K * error */
|
||||
afe_q0 += afe_K[0] * error;
|
||||
afe_q1 += afe_K[1] * error;
|
||||
afe_q2 += afe_K[2] * error;
|
||||
afe_q3 += afe_K[3] * error;
|
||||
afe_bias_p += afe_K[4] * error;
|
||||
afe_bias_q += afe_K[5] * error;
|
||||
afe_bias_r += afe_K[6] * error;
|
||||
|
||||
/* normalize quaternion */
|
||||
AFE_NORM_QUAT();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Do the Kalman filter on the acceleration and compass readings.
|
||||
* This is normally a very simple:
|
||||
*
|
||||
* E = H * P * H' + R
|
||||
* K = P * H' * inv(E)
|
||||
* P = P - K * H * P
|
||||
* X = X + K * error
|
||||
*
|
||||
* We notice that P * H' is used twice, so we can cache the
|
||||
* results of it.
|
||||
*
|
||||
* H represents the Jacobian of measurements to states, which we know
|
||||
* to only have the top four rows filled in since the attitude
|
||||
* measurement does not relate to the gyro bias. This allows us to
|
||||
* ignore parts of PHt
|
||||
*
|
||||
* We also only process one axis at a time to avoid having to perform
|
||||
* the 3x3 matrix inversion.
|
||||
*/
|
||||
|
||||
void afe_update_phi( const FLOAT_T* accel ) {
|
||||
AFE_COMPUTE_H_PHI();
|
||||
FLOAT_T accel_phi = afe_phi_of_accel(accel);
|
||||
FLOAT_T err_phi = accel_phi - afe_phi;
|
||||
AFE_WARP( err_phi, M_PI);
|
||||
run_kalman( AFE_R_PHI, err_phi );
|
||||
AFE_DCM_OF_QUAT();
|
||||
AFE_EULER_OF_DCM();
|
||||
}
|
||||
|
||||
void afe_update_theta( const FLOAT_T* accel ) {
|
||||
AFE_COMPUTE_H_THETA();
|
||||
FLOAT_T accel_theta = afe_theta_of_accel(accel);
|
||||
FLOAT_T err_theta = accel_theta - afe_theta;
|
||||
AFE_WARP( err_theta, M_PI_2);
|
||||
run_kalman( AFE_R_THETA, err_theta );
|
||||
AFE_DCM_OF_QUAT();
|
||||
AFE_EULER_OF_DCM();
|
||||
}
|
||||
|
||||
void afe_update_psi( const int16_t* mag ) {
|
||||
AFE_COMPUTE_H_PSI();
|
||||
FLOAT_T mag_psi = afe_psi_of_mag(mag);
|
||||
FLOAT_T err_psi = mag_psi - afe_psi;
|
||||
AFE_WARP( err_psi, M_PI);
|
||||
run_kalman( AFE_R_PSI, err_psi );
|
||||
AFE_DCM_OF_QUAT();
|
||||
AFE_EULER_OF_DCM();
|
||||
}
|
||||
|
||||
/*
|
||||
* Initialize the AHRS state data and covariance matrix.
|
||||
*/
|
||||
void afe_init( const int16_t *mag, const FLOAT_T* accel, const FLOAT_T* gyro ) {
|
||||
|
||||
/* F and P will be updated only on the non zero locations */
|
||||
memset( (void*) afe_F, 0, sizeof( afe_F ) );
|
||||
memset( (void*) afe_P, 0, sizeof( afe_P ) );
|
||||
int i;
|
||||
for( i=0 ; i<4 ; i++ )
|
||||
afe_P[i][i] = 1.;
|
||||
for( i=4 ; i<7 ; i++ )
|
||||
afe_P[i][i] = .5;
|
||||
|
||||
/* assume vehicle is still, so initial bias are gyro readings */
|
||||
afe_bias_p = gyro[0];
|
||||
afe_bias_q = gyro[1];
|
||||
afe_bias_r = gyro[2];
|
||||
|
||||
afe_phi = afe_phi_of_accel(accel);
|
||||
afe_theta = afe_theta_of_accel(accel);
|
||||
afe_psi = 0.;
|
||||
|
||||
AFE_QUAT_OF_EULER();
|
||||
AFE_DCM_OF_QUAT();
|
||||
afe_psi = afe_psi_of_mag( mag );
|
||||
|
||||
AFE_QUAT_OF_EULER();
|
||||
AFE_DCM_OF_QUAT();
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
#ifndef AHRS_QUAT_FAST_EKF_H
|
||||
#define AHRS_QUAT_FAST_EKF_H
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "6dof.h"
|
||||
|
||||
//#define FLOAT_T double
|
||||
#define FLOAT_T float
|
||||
|
||||
/* ekf state : quaternion and gyro biases */
|
||||
extern FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3;
|
||||
extern FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r;
|
||||
/* we maintain unbiased rates */
|
||||
extern FLOAT_T afe_p, afe_q, afe_r;
|
||||
/* we maintain eulers angles */
|
||||
extern FLOAT_T afe_phi, afe_theta, afe_psi;
|
||||
|
||||
extern FLOAT_T afe_P[7][7]; /* covariance */
|
||||
|
||||
extern void afe_init( const int16_t *mag, const FLOAT_T* accel, const FLOAT_T* gyro );
|
||||
extern void afe_predict( const FLOAT_T* gyro );
|
||||
extern void afe_update_phi( const FLOAT_T* accel);
|
||||
extern void afe_update_theta( const FLOAT_T* accel);
|
||||
extern void afe_update_psi( const int16_t* mag);
|
||||
|
||||
#endif /* AHRS_QUAT_FAST_EKF_H_H */
|
||||
@@ -0,0 +1,80 @@
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "ahrs_quat_fast_ekf.h"
|
||||
#include "ahrs_data.h"
|
||||
#include "ahrs_display.h"
|
||||
|
||||
|
||||
#define UPDATE_PHI 0
|
||||
#define UPDATE_THETA 1
|
||||
#define UPDATE_PSI 2
|
||||
#define UPDATE_NB 3
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
gtk_init(&argc, &argv);
|
||||
|
||||
struct ahrs_data* ad = ahrs_data_read_log("../data/log_ahrs_bug");
|
||||
//struct ahrs_data* ad = ahrs_data_read_log("../data/log_ahrs_roll");
|
||||
//struct ahrs_data* ad = ahrs_data_read_log("../data/log_ahrs_yaw_pitched");
|
||||
|
||||
int idx;
|
||||
int len = 150;
|
||||
double gp=0., gq=0., gr=0., ax = 0., ay=0., az=0., mx=0., my=0., mz=0.;
|
||||
for (idx=0; idx<len; idx++) {
|
||||
gp+=ad->gyro_p[idx]; gq+=ad->gyro_q[idx]; gr+=ad->gyro_r[idx];
|
||||
ax+=ad->accel_x[idx]; ay+=ad->accel_y[idx]; az+=ad->accel_z[idx];
|
||||
mx+=ad->mag_x[idx]; my+=ad->mag_y[idx]; mz+=ad->mag_z[idx];
|
||||
}
|
||||
gp/=len; gq/=len; gr/=len;
|
||||
ax/=len; ay/=len; az/=len;
|
||||
mx/=len; my/=len; mz/=len;
|
||||
int16_t mag_init[3] = {mx, my, mz};
|
||||
FLOAT_T accel_init[3] = {ax, ay, az};
|
||||
FLOAT_T gyro_init[3] = {gp, gq, gr};
|
||||
afe_init(mag_init, accel_init, gyro_init);
|
||||
|
||||
int i;
|
||||
for (i=0; i<ad->nb_samples; i++) {
|
||||
FLOAT_T gyro[3] = {ad->gyro_p[i], ad->gyro_q[i], ad->gyro_r[i]};
|
||||
afe_predict(gyro);
|
||||
int update_stage = i%UPDATE_NB;
|
||||
switch(update_stage) {
|
||||
case UPDATE_PHI: {
|
||||
FLOAT_T accel[3] = {ad->accel_x[i], ad->accel_y[i], ad->accel_z[i]};
|
||||
afe_update_phi(accel);
|
||||
break;
|
||||
}
|
||||
case UPDATE_THETA: {
|
||||
FLOAT_T accel[3] = {ad->accel_x[i], ad->accel_y[i], ad->accel_z[i]};
|
||||
afe_update_theta(accel);
|
||||
break;
|
||||
}
|
||||
case UPDATE_PSI: {
|
||||
int16_t mag[3] = {ad->mag_x[i], ad->mag_y[i], ad->mag_z[i]};
|
||||
afe_update_psi(mag);
|
||||
break;
|
||||
}
|
||||
}
|
||||
/* save state for displaying */
|
||||
double X[] = { afe_q0, afe_q1, afe_q2, afe_q3, afe_bias_p, afe_bias_q, afe_bias_r };
|
||||
double P[] = { afe_P[0][0], afe_P[0][1], afe_P[0][2], afe_P[0][3], afe_P[0][4], afe_P[0][5], afe_P[0][6],
|
||||
afe_P[1][0], afe_P[1][1], afe_P[1][2], afe_P[1][3], afe_P[1][4], afe_P[1][5], afe_P[1][6],
|
||||
afe_P[2][0], afe_P[3][1], afe_P[2][2], afe_P[2][3], afe_P[2][4], afe_P[2][5], afe_P[2][6],
|
||||
afe_P[3][0], afe_P[4][1], afe_P[3][2], afe_P[3][3], afe_P[3][4], afe_P[3][5], afe_P[3][6],
|
||||
afe_P[4][0], afe_P[5][1], afe_P[4][2], afe_P[4][3], afe_P[4][4], afe_P[4][5], afe_P[4][6],
|
||||
afe_P[5][0], afe_P[6][1], afe_P[5][2], afe_P[5][3], afe_P[5][4], afe_P[5][5], afe_P[5][6],
|
||||
afe_P[6][0], afe_P[7][1], afe_P[6][2], afe_P[6][3], afe_P[6][4], afe_P[6][5], afe_P[6][6]};
|
||||
// printf("P66 %f\n", afe_P[6][6]);
|
||||
ahrs_data_save_state(ad, i, X, P);
|
||||
/* save measures for displaying */
|
||||
ahrs_data_save_measure(ad, i);
|
||||
|
||||
}
|
||||
|
||||
ahrs_display(ad);
|
||||
|
||||
gtk_main();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
#ifndef AHRS_QUAT_FAST_EKF_UTILS_H
|
||||
#define AHRS_QUAT__FAST_EKF_UTILS_H
|
||||
|
||||
#include "ahrs_quat_fast_ekf.h"
|
||||
|
||||
/*
|
||||
* Compute the five elements of the DCM that we use for our
|
||||
* rotations and Jacobians. This is used by several other functions
|
||||
* to speedup their computations.
|
||||
*/
|
||||
#define AFE_DCM_OF_QUAT() { \
|
||||
afe_dcm00 = 1.0-2*(afe_q2*afe_q2 + afe_q3*afe_q3); \
|
||||
afe_dcm01 = 2*(afe_q1*afe_q2 + afe_q0*afe_q3); \
|
||||
afe_dcm02 = 2*(afe_q1*afe_q3 - afe_q0*afe_q2); \
|
||||
afe_dcm12 = 2*(afe_q2*afe_q3 + afe_q0*afe_q1); \
|
||||
afe_dcm22 = 1.0-2*(afe_q1*afe_q1 + afe_q2*afe_q2); \
|
||||
}
|
||||
/*
|
||||
* Compute Euler angles from our DCM.
|
||||
*/
|
||||
#define AFE_PHI_OF_DCM() { afe_phi = atan2( afe_dcm12, afe_dcm22 );}
|
||||
#define AFE_THETA_OF_DCM() { afe_theta = -asin( afe_dcm02 );}
|
||||
#define AFE_PSI_OF_DCM() { afe_psi = atan2( afe_dcm01, afe_dcm00 );}
|
||||
#define AFE_EULER_OF_DCM() { AFE_PHI_OF_DCM(); AFE_THETA_OF_DCM(); AFE_PSI_OF_DCM()}
|
||||
|
||||
/*
|
||||
* initialise the quaternion from the set of eulers
|
||||
*/
|
||||
#define AFE_QUAT_OF_EULER() { \
|
||||
const FLOAT_T phi2 = afe_phi / 2.0; \
|
||||
const FLOAT_T theta2 = afe_theta / 2.0; \
|
||||
const FLOAT_T psi2 = afe_psi / 2.0; \
|
||||
\
|
||||
const FLOAT_T sinphi2 = sin( phi2 ); \
|
||||
const FLOAT_T cosphi2 = cos( phi2 ); \
|
||||
\
|
||||
const FLOAT_T sintheta2 = sin( theta2 ); \
|
||||
const FLOAT_T costheta2 = cos( theta2 ); \
|
||||
\
|
||||
const FLOAT_T sinpsi2 = sin( psi2 ); \
|
||||
const FLOAT_T cospsi2 = cos( psi2 ); \
|
||||
\
|
||||
afe_q0 = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; \
|
||||
afe_q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; \
|
||||
afe_q2 = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; \
|
||||
afe_q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; \
|
||||
}
|
||||
|
||||
/*
|
||||
* normalize quaternion
|
||||
*/
|
||||
#define AFE_NORM_QUAT() { \
|
||||
FLOAT_T mag = afe_q0*afe_q0 + afe_q1*afe_q1 \
|
||||
+ afe_q2*afe_q2 + afe_q3*afe_q3; \
|
||||
mag = sqrt( mag ); \
|
||||
afe_q0 /= mag; \
|
||||
afe_q1 /= mag; \
|
||||
afe_q2 /= mag; \
|
||||
afe_q3 /= mag; \
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Compute the Jacobian of the measurements to the system states.
|
||||
*/
|
||||
#define AFE_COMPUTE_H_PHI() { \
|
||||
const FLOAT_T phi_err = 2 / (afe_dcm22*afe_dcm22 + afe_dcm12*afe_dcm12); \
|
||||
afe_H[0] = (afe_q1 * afe_dcm22) * phi_err; \
|
||||
afe_H[1] = (afe_q0 * afe_dcm22 + 2 * afe_q1 * afe_dcm12) * phi_err; \
|
||||
afe_H[2] = (afe_q3 * afe_dcm22 + 2 * afe_q2 * afe_dcm12) * phi_err; \
|
||||
afe_H[3] = (afe_q2 * afe_dcm22) * phi_err; \
|
||||
}
|
||||
|
||||
#define AFE_COMPUTE_H_THETA() { \
|
||||
const FLOAT_T theta_err = -2 / sqrt( 1 - afe_dcm02*afe_dcm02 ); \
|
||||
afe_H[0] = -afe_q2 * theta_err; \
|
||||
afe_H[1] = afe_q3 * theta_err; \
|
||||
afe_H[2] = -afe_q0 * theta_err; \
|
||||
afe_H[3] = afe_q1 * theta_err; \
|
||||
}
|
||||
|
||||
#define AFE_COMPUTE_H_PSI() { \
|
||||
const FLOAT_T psi_err = 2 / (afe_dcm00*afe_dcm00 + afe_dcm01*afe_dcm01); \
|
||||
afe_H[0] = (afe_q3 * afe_dcm00) * psi_err; \
|
||||
afe_H[1] = (afe_q2 * afe_dcm00) * psi_err; \
|
||||
afe_H[2] = (afe_q1 * afe_dcm00 + 2 * afe_q2 * afe_dcm01) * psi_err; \
|
||||
afe_H[3] = (afe_q0 * afe_dcm00 + 2 * afe_q3 * afe_dcm01) * psi_err; \
|
||||
}
|
||||
|
||||
|
||||
/* convert sensor reading into euler angle measurement */
|
||||
static inline FLOAT_T afe_phi_of_accel( const FLOAT_T* accel ) {
|
||||
return atan2(accel[AXIS_Y], accel[AXIS_Z]);
|
||||
}
|
||||
|
||||
static inline FLOAT_T afe_theta_of_accel( const FLOAT_T* accel) {
|
||||
FLOAT_T g2 =
|
||||
accel[AXIS_X]*accel[AXIS_X] +
|
||||
accel[AXIS_Y]*accel[AXIS_Y] +
|
||||
accel[AXIS_Z]*accel[AXIS_Z];
|
||||
return -asin( accel[AXIS_X] / sqrt( g2 ) );
|
||||
}
|
||||
/*
|
||||
* The rotation matrix to rotate from NED frame to body frame without
|
||||
* rotating in the yaw axis is:
|
||||
*
|
||||
* [ 1 0 0 ] [ cos(Theta) 0 -sin(Theta) ]
|
||||
* [ 0 cos(Phi) sin(Phi) ] [ 0 1 0 ]
|
||||
* [ 0 -sin(Phi) cos(Phi) ] [ sin(Theta) 0 cos(Theta) ]
|
||||
*
|
||||
* This expands to:
|
||||
*
|
||||
* [ cos(Theta) 0 -sin(Theta) ]
|
||||
* [ sin(Phi)*sin(Theta) cos(Phi) sin(Phi)*cos(Theta)]
|
||||
* [ cos(Phi)*sin(Theta) -sin(Phi) cos(Phi)*cos(Theta)]
|
||||
*
|
||||
* However, to untilt the compass reading, we need to use the
|
||||
* transpose of this matrix.
|
||||
*
|
||||
* [ cos(Theta) sin(Phi)*sin(Theta) cos(Phi)*sin(Theta) ]
|
||||
* [ 0 cos(Phi) -sin(Phi) ]
|
||||
* [ -sin(Theta) sin(Phi)*cos(Theta) cos(Phi)*cos(Theta) ]
|
||||
*
|
||||
* Additionally,
|
||||
* since we already have the DCM computed for our current attitude,
|
||||
* we can short cut all of the trig. substituting
|
||||
* in from the definition of euler2quat and quat2euler, we have:
|
||||
*
|
||||
* [ cos(Theta) -dcm12*dcm02 -dcm22*dcm02 ]
|
||||
* [ 0 dcm22 -dcm12 ]
|
||||
* [ dcm02 dcm12 dcm22 ]
|
||||
*
|
||||
*/
|
||||
static inline FLOAT_T afe_psi_of_mag( const int16_t* mag ) {
|
||||
const FLOAT_T ctheta = cos( afe_theta );
|
||||
#if 0
|
||||
const FLOAT_T mn = ctheta * mag[0]
|
||||
- (afe_dcm12 * mag[1] + afe_dcm22 * mag[2]) * afe_dcm02 / ctheta;
|
||||
|
||||
const FLOAT_T me =
|
||||
(afe_dcm22 * mag[1] - afe_dcm12 * mag[2]) / ctheta;
|
||||
#else
|
||||
const FLOAT_T stheta = sin( afe_theta );
|
||||
const FLOAT_T cphi = cos( afe_phi );
|
||||
const FLOAT_T sphi = sin( afe_phi );
|
||||
|
||||
const FLOAT_T mn =
|
||||
ctheta* mag[0]+
|
||||
sphi*stheta* mag[1]+
|
||||
cphi*stheta* mag[2];
|
||||
const FLOAT_T me =
|
||||
0* mag[0]+
|
||||
cphi* mag[1]+
|
||||
-sphi* mag[2];
|
||||
#endif
|
||||
|
||||
const FLOAT_T psi = -atan2( me, mn );
|
||||
return psi;
|
||||
}
|
||||
|
||||
#define AFE_WARP(x, b) { \
|
||||
while( x < -b ) \
|
||||
x += 2 * b; \
|
||||
while( x > b ) \
|
||||
x -= 2 * b; \
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* AHRS_QUAT_FAST_EKF_UTILS_H */
|
||||
@@ -0,0 +1,123 @@
|
||||
#include "ahrs_data.h"
|
||||
#include "ahrs_display.h"
|
||||
#include "ahrs_utils.h"
|
||||
#include "ukf.h"
|
||||
|
||||
#define UPDATE_PHI 0
|
||||
#define UPDATE_THETA 1
|
||||
#define UPDATE_PSI 2
|
||||
#define UPDATE_NB 3
|
||||
|
||||
static struct ahrs_data* ad;
|
||||
static int ahrs_state;
|
||||
|
||||
void linear_filter(double *x1, double *x0, double *u) {
|
||||
/*
|
||||
* quat_dot = Wxq(pqr) * quat
|
||||
* bias_dot = 0
|
||||
*
|
||||
* Wxq is the quaternion omega matrix:
|
||||
*
|
||||
* [ 0, -p, -q, -r ]
|
||||
* 1/2 * [ p, 0, r, -q ]
|
||||
* [ q, -r, 0, p ]
|
||||
* [ r, q, -p, 0 ]
|
||||
*/
|
||||
double p = u[0] - x0[4];
|
||||
double q = u[1] - x0[5];
|
||||
double r = u[2] - x0[6];
|
||||
double q0_dot = -p*x0[1] -q*x0[2] -r*x0[3];
|
||||
double q1_dot = p*x0[0] +r*x0[2] -q*x0[3];
|
||||
double q2_dot = q*x0[0] -r*x0[1] +p*x0[3];
|
||||
double q3_dot = r*x0[0] +q*x0[1] -p*x0[2];
|
||||
|
||||
x1[0] = x0[0] + q0_dot * ad->dt;
|
||||
x1[1] = x0[1] + q1_dot * ad->dt;
|
||||
x1[2] = x0[2] + q2_dot * ad->dt;
|
||||
x1[3] = x0[3] + q3_dot * ad->dt;
|
||||
x1[4] = x0[4];
|
||||
x1[5] = x0[5];
|
||||
x1[6] = x0[6];
|
||||
|
||||
norm_quat(x1);
|
||||
}
|
||||
|
||||
void linear_measure(double *y, double *x) {
|
||||
double eulers[3];
|
||||
eulers_of_quat(eulers, x);
|
||||
y[0] = eulers[ahrs_state];
|
||||
}
|
||||
|
||||
|
||||
|
||||
void run_ukf(void) {
|
||||
/* initial state covariance matrix */
|
||||
double P[7*7] = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
/* model noise covariance matrix */
|
||||
double Q[7*7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.008, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.008, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.008 };
|
||||
/* measurement noise covariance matrix */
|
||||
double R[1]={0.3};
|
||||
/* state [q0, q1, q2, q3, bp, bq, br] */
|
||||
double X[7];
|
||||
/* measure */
|
||||
double Y[1];
|
||||
/* command */
|
||||
double U[3] = {0.0, 0.0, 0.0};
|
||||
|
||||
ukf_filter filter;
|
||||
filter = ukf_filter_new(7, 1, Q, R, linear_filter, linear_measure);
|
||||
ahrs_init(ad, 150, X);
|
||||
ukf_filter_reset(filter, X, P);
|
||||
ukf_filter_compute_weights(filter, 1.1, 0.0, 2.0);
|
||||
|
||||
int iter;
|
||||
for (iter=0; iter < ad->nb_samples; iter++) {
|
||||
U[0] = ad->gyro_p[iter];// - X[4];
|
||||
U[1] = ad->gyro_q[iter];// - X[5];
|
||||
U[2] = ad->gyro_r[iter];// - X[6];
|
||||
ahrs_state = iter%3;
|
||||
switch (ahrs_state) {
|
||||
case UPDATE_PHI:
|
||||
Y[0] = phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
|
||||
break;
|
||||
case UPDATE_THETA:
|
||||
Y[0] = theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
|
||||
break;
|
||||
case UPDATE_PSI: {
|
||||
double eulers[3];
|
||||
eulers_of_quat(eulers, X);
|
||||
Y[0] = psi_of_mag(eulers[0], eulers[1], ad->mag_x[iter], ad->mag_y[iter], ad->mag_z[iter]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
ukf_filter_update(filter, Y, U);
|
||||
ukf_filter_get_state(filter, X, P);
|
||||
ahrs_data_save_state(ad, iter, X, P);
|
||||
}
|
||||
|
||||
ukf_filter_delete(filter);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
gtk_init(&argc, &argv);
|
||||
|
||||
//ad = ahrs_data_read_log("../data/log_ahrs_bug");
|
||||
ad = ahrs_data_read_log("../data/log_ahrs_roll");
|
||||
// ad = ahrs_data_read_log("../data/log_ahrs_yaw_pitched");
|
||||
run_ukf();
|
||||
ahrs_display(ad);
|
||||
gtk_main();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,118 @@
|
||||
#include "ahrs_utils.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
extern double phi_of_accel(double an, double ae, double ad) {
|
||||
return atan2(ae, ad);
|
||||
}
|
||||
|
||||
extern double theta_of_accel(double an, double ae, double ad) {
|
||||
const double g2 = an * an + ae * ae + ad * ad;
|
||||
return -asin(an / sqrt(g2));
|
||||
}
|
||||
|
||||
extern double psi_of_mag(double phi, double theta, double mx, double my, double mz) {
|
||||
const float cphi = cos( phi );
|
||||
const float sphi = sin( phi );
|
||||
const float ctheta = cos( theta );
|
||||
const float stheta = sin( theta );
|
||||
const float mn =
|
||||
ctheta * mx +
|
||||
sphi * stheta * my +
|
||||
cphi * stheta * mz;
|
||||
const float me =
|
||||
0 * mx+
|
||||
cphi * my+
|
||||
-sphi * mz;
|
||||
return -atan2( me, mn );
|
||||
}
|
||||
|
||||
void quat_of_eulers (double* quat, double phi, double theta, double psi ) {
|
||||
const float phi2 = phi / 2.0;
|
||||
const float theta2 = theta / 2.0;
|
||||
const float psi2 = psi / 2.0;
|
||||
|
||||
const float sinphi2 = sin( phi2 );
|
||||
const float cosphi2 = cos( phi2 );
|
||||
|
||||
const float sintheta2 = sin( theta2 );
|
||||
const float costheta2 = cos( theta2 );
|
||||
|
||||
const float sinpsi2 = sin( psi2 );
|
||||
const float cospsi2 = cos( psi2 );
|
||||
|
||||
quat[0] = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2;
|
||||
quat[1] = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2;
|
||||
quat[2] = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
|
||||
quat[3] = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
|
||||
}
|
||||
|
||||
void eulers_of_quat(double* euler, double* quat) {
|
||||
double dcm00 = 1.0-2*(quat[2]*quat[2] + quat[3]*quat[3]);
|
||||
double dcm01 = 2*(quat[1]*quat[2] + quat[0]*quat[3]);
|
||||
double dcm02 = 2*(quat[1]*quat[3] - quat[0]*quat[2]);
|
||||
double dcm12 = 2*(quat[2]*quat[3] + quat[0]*quat[1]);
|
||||
double dcm22 = 1.0-2*(quat[1]*quat[1] + quat[2]*quat[2]);
|
||||
|
||||
euler[0] = atan2( dcm12, dcm22 );
|
||||
euler[1] = -asin( dcm02 );
|
||||
euler[2] = atan2( dcm01, dcm00 );
|
||||
}
|
||||
|
||||
void norm_quat(double* quat) {
|
||||
double mag = quat[0]*quat[0] + quat[1]*quat[1] +
|
||||
quat[2]*quat[2] + quat[3]*quat[3];
|
||||
mag = sqrt( mag );
|
||||
quat[0] /= mag;
|
||||
quat[1] /= mag;
|
||||
quat[2] /= mag;
|
||||
quat[3] /= mag;
|
||||
}
|
||||
|
||||
void ahrs_euler_init(struct ahrs_data* ad, int len, double* X) {
|
||||
double init_phi = 0.;
|
||||
double init_theta = 0.;
|
||||
double init_mx = 0.;
|
||||
double init_my = 0.;
|
||||
double init_mz = 0.;
|
||||
X[3] = 0.;
|
||||
X[4] = 0.;
|
||||
X[5] = 0.;
|
||||
int iter;
|
||||
for (iter = 0; iter < len; iter++) {
|
||||
/* average attitude */
|
||||
init_phi += phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
|
||||
init_theta += theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
|
||||
init_mx += ad->mag_x[iter];
|
||||
init_my += ad->mag_y[iter];
|
||||
init_mz += ad->mag_z[iter];
|
||||
/* sum gyros */
|
||||
X[3] += ad->gyro_p[iter];
|
||||
X[4] += ad->gyro_q[iter];
|
||||
X[5] += ad->gyro_r[iter];
|
||||
}
|
||||
init_phi /= (double)len;
|
||||
init_theta /= (double)len;
|
||||
init_mx /= (double)len;
|
||||
init_my /= (double)len;
|
||||
init_mz /= (double)len;
|
||||
double init_psi = psi_of_mag(init_phi, init_theta, init_mx, init_my, init_mz);
|
||||
X[0] = init_phi;
|
||||
X[1] = init_theta;
|
||||
X[2] = init_psi;
|
||||
X[3] /= (double)len;
|
||||
X[4] /= (double)len;
|
||||
X[5] /= (double)len;
|
||||
printf("ahrs init : eulers [%f %f %f] biases [%f %f %f]\n", init_phi, init_theta, init_psi, X[3], X[4], X[5]);
|
||||
}
|
||||
|
||||
void ahrs_quat_init(struct ahrs_data* ad, int len, double* X) {
|
||||
ahrs_euler_init(ad, len, X);
|
||||
|
||||
X[6] = X[5];
|
||||
X[5] = X[4];
|
||||
X[4] = X[3];
|
||||
quat_of_eulers(X, X[0], X[1], X[2]);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
#ifndef AHRS_UTILS_H
|
||||
#define AHRS_UTILS_H
|
||||
|
||||
#include "ahrs_data.h"
|
||||
|
||||
extern double phi_of_accel(double an, double ae, double ad);
|
||||
extern double theta_of_accel(double an, double ae, double ad);
|
||||
extern double psi_of_mag(double phi, double theta, double mx, double my, double mz);
|
||||
extern void quat_of_eulers(double* quat, double phi, double theta, double psi );
|
||||
extern void eulers_of_quat(double* euler, double* quat);
|
||||
extern void norm_quat(double* quat);
|
||||
|
||||
extern void ahrs_quat_init(struct ahrs_data* ad, int len, double* X);
|
||||
extern void ahrs_euler_init(struct ahrs_data* ad, int len, double* X);
|
||||
|
||||
#define WARP(x, y) { \
|
||||
while ( x < -y ) \
|
||||
x += 2 * y; \
|
||||
while ( x > y ) \
|
||||
x -= 2 * y; \
|
||||
}
|
||||
|
||||
|
||||
#endif /* AHRS_UTILS_H */
|
||||
@@ -0,0 +1,182 @@
|
||||
#include "ekf.h"
|
||||
|
||||
#include "matrix.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
struct ekf_filter {
|
||||
unsigned state_dim;
|
||||
unsigned measure_dim;
|
||||
|
||||
/* state */
|
||||
double* X;
|
||||
/* state covariance matrix */
|
||||
double* P;
|
||||
/* process covariance noise */
|
||||
double* Q;
|
||||
/* measurement covariance noise */
|
||||
double* R;
|
||||
/* jacobian of Xdot wrt X */
|
||||
double* F;
|
||||
/* jacobian of the measure wrt X */
|
||||
double* H;
|
||||
/* error matrix */
|
||||
double* E;
|
||||
/* kalman gain */
|
||||
double* K;
|
||||
|
||||
filter_function ffun;
|
||||
measure_function mfun;
|
||||
|
||||
/* temps */
|
||||
double* Xdot;
|
||||
double* Pdot;
|
||||
double* tmp1;
|
||||
double* tmp2;
|
||||
double* tmp3;
|
||||
|
||||
};
|
||||
|
||||
|
||||
struct ekf_filter* ekf_filter_new(unsigned state_dim,
|
||||
unsigned measure_dim,
|
||||
double* Q,
|
||||
double* R,
|
||||
filter_function ffun,
|
||||
measure_function mfun) {
|
||||
|
||||
struct ekf_filter* ekf = malloc(sizeof(struct ekf_filter));
|
||||
ekf->state_dim = state_dim;
|
||||
ekf->measure_dim = measure_dim;
|
||||
|
||||
int n = ekf->state_dim;
|
||||
ekf->X = malloc( n * sizeof(double));
|
||||
ekf->Xdot = malloc( n * sizeof(double));
|
||||
|
||||
n = ekf->state_dim * ekf->state_dim;
|
||||
ekf->P = malloc( n * sizeof(double));
|
||||
ekf->Pdot = malloc( n * sizeof(double));
|
||||
ekf->tmp1 = malloc( n * sizeof(double));
|
||||
ekf->tmp2 = malloc( n * sizeof(double));
|
||||
ekf->tmp3 = malloc( n * sizeof(double));
|
||||
|
||||
ekf->Q = malloc( n * sizeof(double));
|
||||
memcpy(ekf->Q, Q, n * sizeof(double));
|
||||
|
||||
n = ekf->measure_dim * ekf->measure_dim;
|
||||
ekf->R = malloc( n * sizeof(double));
|
||||
memcpy(ekf->R, R, n * sizeof(double));
|
||||
|
||||
n = ekf->state_dim * ekf->state_dim;
|
||||
ekf->F = malloc( n * sizeof(double));
|
||||
|
||||
n = ekf->measure_dim * ekf->state_dim;
|
||||
ekf->H = malloc( n * sizeof(double));
|
||||
|
||||
n = ekf->measure_dim * ekf->measure_dim;
|
||||
ekf->E = malloc( n * sizeof(double));
|
||||
|
||||
n = ekf->state_dim;
|
||||
ekf->K = malloc( n * sizeof(double));
|
||||
|
||||
|
||||
ekf->ffun = ffun;
|
||||
ekf->mfun = mfun;
|
||||
|
||||
return ekf;
|
||||
}
|
||||
|
||||
|
||||
void ekf_filter_reset(struct ekf_filter *filter, double *x0, double *P0) {
|
||||
memcpy(filter->X, x0, filter->state_dim * sizeof(double));
|
||||
memcpy(filter->P, P0, filter->state_dim * filter->state_dim * sizeof(double));
|
||||
}
|
||||
|
||||
void
|
||||
ekf_filter_get_state(struct ekf_filter* filter, double *X, double* P){
|
||||
memcpy(X, filter->X, filter->state_dim * sizeof(double));
|
||||
memcpy(P, filter->P, filter->state_dim * filter->state_dim * sizeof(double));
|
||||
}
|
||||
|
||||
void ekf_filter_predict(struct ekf_filter* filter, double *u) {
|
||||
|
||||
/* prediction :
|
||||
|
||||
X += Xdot * dt
|
||||
Pdot = F * P * F' + Q ( or Pdot = F*P + P*F' + Q for continuous form )
|
||||
P += Pdot * dt
|
||||
|
||||
*/
|
||||
|
||||
int n = filter->state_dim;
|
||||
double dt;
|
||||
/* fetch dt, Xdot and F */
|
||||
filter->ffun(u, filter->X, &dt, filter->Xdot, filter->F);
|
||||
/* X = X + Xdot * dt */
|
||||
mat_add_scal_mult(n, 1, filter->X, filter->X, dt, filter->Xdot);
|
||||
|
||||
#ifdef EKF_UPDATE_CONTINUOUS
|
||||
/*
|
||||
continuous update
|
||||
Pdot = F * P + P * F' + Q
|
||||
*/
|
||||
mat_mult(n, n, n, filter->tmp1, filter->F, filter->P);
|
||||
mat_transpose(n, n, filter->tmp2, filter->F);
|
||||
mat_mult(n, n, n, filter->tmp3, filter->P, filter->tmp2);
|
||||
mat_add(n, n, filter->Pdot, filter->tmp1, filter->tmp3);
|
||||
mat_add(n, n, filter->Pdot, filter->Pdot, filter->Q);
|
||||
#endif
|
||||
#ifdef EKF_UPDATE_DISCRETE
|
||||
/*
|
||||
discrete update
|
||||
Pdot = F * P * F' + Q
|
||||
*/
|
||||
mat_mult(n, n, n, filter->tmp1, filter->F, filter->P);
|
||||
mat_transpose(n, n, filter->tmp2, filter->F);
|
||||
mat_mult(n, n, n, filter->tmp3, filter->tmp1, filter->tmp2);
|
||||
mat_add(n, n, filter->Pdot, filter->tmp3, filter->Q);
|
||||
#endif
|
||||
|
||||
/* P = P + Pdot * dt */
|
||||
mat_add_scal_mult(n, n, filter->P, filter->P, dt, filter->Pdot);
|
||||
|
||||
}
|
||||
|
||||
void ekf_filter_update(struct ekf_filter* filter, double *y) {
|
||||
|
||||
/* update
|
||||
|
||||
E = H * P * H' + R
|
||||
K = P * H' * inv(E)
|
||||
P = P - K * H * P
|
||||
X = X + K * err
|
||||
|
||||
*/
|
||||
double err;
|
||||
int n = filter->state_dim;
|
||||
int m = filter->measure_dim;
|
||||
filter->mfun(y, &err, filter->X, filter->H);
|
||||
|
||||
/* E = H * P * H' + R */
|
||||
mat_mult(m, n, n, filter->tmp1, filter->H, filter->P);
|
||||
mat_transpose(m, n, filter->tmp2, filter->H);
|
||||
mat_mult(m, n, m, filter->tmp3, filter->tmp1, filter->tmp2);
|
||||
mat_add(m, m, filter->E, filter->tmp3, filter->R);
|
||||
|
||||
/* K = P * H' * inv(E) */
|
||||
mat_transpose(m, n, filter->tmp1, filter->H);
|
||||
mat_mult(n, n, m, filter->tmp2, filter->P, filter->tmp1);
|
||||
if (filter->measure_dim != 1) { printf("only dim 1 measure implemented for now\n"); exit(-1);}
|
||||
mat_scal_mult(n, m, filter->K, 1./filter->E[0], filter->tmp2);
|
||||
|
||||
/* P = P - K * H * P */
|
||||
mat_mult(n, m, n, filter->tmp1, filter->K, filter->H);
|
||||
mat_mult(n, n, n, filter->tmp2, filter->tmp1, filter->P);
|
||||
mat_sub(n, n, filter->P, filter->P, filter->tmp2);
|
||||
|
||||
/* X = X + err * K */
|
||||
mat_add_scal_mult(n, m, filter->X, filter->X, err, filter->K);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
#ifndef EKF_H
|
||||
#define EKF_H
|
||||
|
||||
struct ekf_filter;
|
||||
|
||||
/*
|
||||
* Basic functions describing evolution and measure
|
||||
*/
|
||||
typedef void (*filter_function)(double*, double*, double *,double *, double*);
|
||||
typedef void (*measure_function)(double *, double *, double*, double*);
|
||||
|
||||
extern struct ekf_filter* ekf_filter_new(unsigned state_dim,
|
||||
unsigned measure_dim,
|
||||
double* Q,
|
||||
double* R,
|
||||
filter_function ffun,
|
||||
measure_function mfun);
|
||||
extern void ekf_filter_reset(struct ekf_filter *filter, double *x0, double *P0);
|
||||
|
||||
extern void ekf_filter_predict(struct ekf_filter *filter, double *u);
|
||||
extern void ekf_filter_update(struct ekf_filter *filter, double *y);
|
||||
|
||||
extern void ekf_filter_get_state(struct ekf_filter* filter, double *X, double* P);
|
||||
#endif /* EKF_H */
|
||||
@@ -0,0 +1,67 @@
|
||||
#include<math.h>
|
||||
#include<stdio.h>
|
||||
#include"linalg.h"
|
||||
|
||||
void
|
||||
ukf_cholesky_decomposition(double *A, unsigned n, double *sigma) {
|
||||
unsigned i,j,k;
|
||||
double t;
|
||||
|
||||
for(i = 0 ; i < n ; i++) {
|
||||
if(i > 0) {
|
||||
for(j = i ; j < n ; j++) {
|
||||
t = 0.0;
|
||||
for(k = 0 ; k < i ; k++)
|
||||
t += A[j * n + k] * A[i * n + k];
|
||||
A[j * n + i] -= t;
|
||||
}
|
||||
}
|
||||
if(A[i * n + i] <= 0.0) {
|
||||
fprintf(stderr, "Matrix is not positive definite \n");
|
||||
return;
|
||||
}
|
||||
sigma[i] = sqrt(A[i * n + i]);
|
||||
t = 1.0 / sigma[i];
|
||||
for(j = i ; j < n ; j++)
|
||||
A[j * n + i] *= t;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
ukf_cholesky_solve(double *A, unsigned n, double *sigma, double *B, unsigned m, double *X) {
|
||||
int i,j,k;
|
||||
double t;
|
||||
|
||||
for(i = 0 ; i < m ; i++) { // iterate over the lines of B
|
||||
for(j = 0 ; j < n ; j++) { // solve Ly=B
|
||||
t = B[i * n + j];
|
||||
for(k = j - 1 ; k >= 0 ; k--)
|
||||
t -= A[j * n + k] * X[i * n + k];
|
||||
X[i * n + j] = t / sigma[j];
|
||||
}
|
||||
for(j = n - 1 ; j >= 0 ; j--) { // solve Ltx=y
|
||||
t = X[i * n + j];
|
||||
for(k = j + 1 ; k < n ; k++)
|
||||
t -= A[k * n + j] * X[i * n + k];
|
||||
X[i * n + j] = t / sigma[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
ukf_cholesky_invert(double *A, unsigned n, double *sigma) {
|
||||
double t;
|
||||
int i,j,k;
|
||||
|
||||
for(i = 0 ; i < n ; i++) {
|
||||
A[i * n + i] = 1.0 / sigma[i];
|
||||
for(j = i + 1 ; j < n ; j++) {
|
||||
t = 0.0;
|
||||
for(k = i ; k < j ; k++)
|
||||
t -= A[j * n + k] * A[k * n + i];
|
||||
A[j * n + i] = t / sigma[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
#ifndef LINALG_H_
|
||||
#define LINALG_H_
|
||||
|
||||
/*
|
||||
* Computes cholesky decomposition of A
|
||||
* @param A: a n x n matrix. On exit, the lower triangle
|
||||
* of A is overwritten with the cholesky decomposition
|
||||
* @param n: the dimension of the square matrix A
|
||||
* @param sigma: the vector of diagonal elements of the cholesky
|
||||
* decomposition
|
||||
*/
|
||||
|
||||
void
|
||||
ukf_cholesky_decomposition(double *A, unsigned n, double *sigma);
|
||||
|
||||
/*
|
||||
* Solve the linear system AX^t=B^t given the cholesky decomposition of A
|
||||
* @param A: the cholesky decomposition of A, as given by the
|
||||
* routine ukf_cholesky_decomposition
|
||||
* @param n: the size of the problem
|
||||
* @param sigma: the vector of diagonal elements of A
|
||||
* @param B: the right hand side of the linear system. Size n x m
|
||||
* @param m: the number of simultaneou systems to solve
|
||||
* @param X: a matrix holding the result
|
||||
*/
|
||||
|
||||
void
|
||||
ukf_cholesky_solve(double *A, unsigned n, double *sigma, double *B, unsigned m, double *X);
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Lower triangle of the inverse of the cholesky decomposition.
|
||||
* On exit, the lower triangle of A is overwritten with the lower
|
||||
* triangle of the inverse.
|
||||
* @param A: the cholesky decomposition of A, as given by the
|
||||
* routine ukf_cholesky_decomposition
|
||||
* @param n: the size of the problem
|
||||
* @param sigma: the vector of diagonal elements of A
|
||||
*/
|
||||
void
|
||||
ukf_cholesky_invert(double *A, unsigned n, double *sigma);
|
||||
|
||||
#endif /*LINALG_H_*/
|
||||
@@ -0,0 +1,80 @@
|
||||
#include "matrix.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
void mat_add(int n_row, int n_col, double* r, double* a, double* b) {
|
||||
int row, col;
|
||||
for (row = 0; row<n_row; row++) {
|
||||
for (col = 0; col<n_col; col++) {
|
||||
int ridx = row * n_col + col;
|
||||
r[ridx] = a[ridx] + b[ridx];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mat_sub(int n_row, int n_col, double* r, double* a, double* b) {
|
||||
int row, col;
|
||||
for (row = 0; row<n_row; row++) {
|
||||
for (col = 0; col<n_col; col++) {
|
||||
int ridx = row * n_col + col;
|
||||
r[ridx] = a[ridx] - b[ridx];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mat_transpose(int n_row, int n_col, double* r, double* a) {
|
||||
int row, col;
|
||||
for (row = 0; row<n_row; row++) {
|
||||
for (col = 0; col<n_col; col++) {
|
||||
int aidx = row * n_col + col;
|
||||
int ridx = col * n_row + row;
|
||||
r[ridx] = a[aidx];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mat_scal_mult(int n_row, int n_col, double* r, double k, double* a) {
|
||||
int row, col;
|
||||
for (row = 0; row<n_row; row++) {
|
||||
for (col = 0; col<n_col; col++) {
|
||||
int ridx = row * n_col + col;
|
||||
r[ridx] = k * a[ridx];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mat_add_scal_mult(int n_row, int n_col, double* r, double*a, double k, double* b) {
|
||||
int row, col;
|
||||
for (row = 0; row<n_row; row++) {
|
||||
for (col = 0; col<n_col; col++) {
|
||||
int ridx = row * n_col + col;
|
||||
r[ridx] = a[ridx] + k * b[ridx];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mat_mult(int n_rowa, int n_cola, int n_colb, double* r, double* a, double* b) {
|
||||
int row, col, k;
|
||||
for (row = 0; row<n_rowa; row++) {
|
||||
for (col = 0; col<n_colb; col++) {
|
||||
int ridx = col + row * n_colb;
|
||||
r[ridx] =0.;
|
||||
for (k=0; k<n_cola; k++) {
|
||||
int aidx = k + row * n_cola;
|
||||
int bidx = col + k * n_colb;
|
||||
r[ridx] += a[aidx] * b[bidx];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mat_print(int n_row, int n_col, double* a) {
|
||||
int row, col;
|
||||
for (row = 0; row<n_row; row++) {
|
||||
for (col = 0; col<n_col; col++) {
|
||||
int ridx = row * n_col + col;
|
||||
printf("%lf\t", a[ridx]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#ifndef MATRIX_H
|
||||
#define MATRIX_H
|
||||
|
||||
extern void mat_add(int n_row, int n_col, double* r, double* a, double* b);
|
||||
extern void mat_sub(int n_row, int n_col, double* r, double* a, double* b);
|
||||
extern void mat_transpose(int n_row, int n_col, double* r, double* a);
|
||||
extern void mat_scal_mult(int n_row, int n_col, double* r, double k, double* a);
|
||||
extern void mat_add_scal_mult(int n_row, int n_col, double* r, double*a, double k, double* b);
|
||||
extern void mat_mult(int n_rowa, int n_cola, int n_colb, double* r, double* a, double* b);
|
||||
|
||||
extern void mat_print(int n_row, int n_col, double* a);
|
||||
|
||||
#endif /* MATRIX_H */
|
||||
@@ -0,0 +1,27 @@
|
||||
#include"random.h"
|
||||
#include<stdlib.h>
|
||||
#include<math.h>
|
||||
|
||||
double
|
||||
ukf_filter_rand_normal() {
|
||||
static int is_ready = 0;
|
||||
double norm2,x,y;
|
||||
double bm;
|
||||
double saved_value;
|
||||
|
||||
if(is_ready == 0) {
|
||||
do {
|
||||
x = 2.0 * drand48() - 1.0;
|
||||
y = 2.0 * drand48() - 1.0;
|
||||
norm2 = x * x + y * y;
|
||||
} while(norm2 >= 1.0 || norm2 == 0.0);
|
||||
bm = sqrt(-2.0 * log(norm2) / norm2);
|
||||
saved_value = x * bm;
|
||||
is_ready = 1;
|
||||
return y * bm;
|
||||
} else {
|
||||
is_ready = 0;
|
||||
return saved_value;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
#ifndef RANDOM_H_
|
||||
#define RANDOM_H_
|
||||
/*
|
||||
* Random number generation
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
* Generates a random gaussian number
|
||||
*/
|
||||
double
|
||||
ukf_filter_rand_normal();
|
||||
#endif /*RANDOM_H_*/
|
||||
@@ -0,0 +1,159 @@
|
||||
#include "tilt_data.h"
|
||||
|
||||
#include "random.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <glib.h>
|
||||
|
||||
#define DT 0.01
|
||||
#define T_END 90.
|
||||
#define NB_POINTS (int)(T_END/DT)
|
||||
#define T0 10.
|
||||
#define T1 11.
|
||||
#define T2 12.
|
||||
#define DELTA_T1 (T1 - T0)
|
||||
#define DELTA_T2 (T2 - T1)
|
||||
#define MAX_RATE 0.75
|
||||
|
||||
#define NOISE_GYRO 0.05
|
||||
#define BIAS_GYRO 0.02
|
||||
#define DBIAS_GYRO 0.005
|
||||
#define NOISE_ACCEL 1.
|
||||
#define G 9.81
|
||||
|
||||
struct tilt_data* tilt_data_gen(void) {
|
||||
struct tilt_data* td = tilt_data_new(NB_POINTS, DT);
|
||||
int i;
|
||||
for (i=0; i< NB_POINTS; i++) {
|
||||
td->t[i] = i * DT;
|
||||
if ((td->t[i] < T0) | (td->t[i] > T2)) {
|
||||
td->rate[i] = 0.;
|
||||
td->angle[i] = 0.;
|
||||
}
|
||||
else {
|
||||
if ( td->t[i] < T1)
|
||||
td->rate[i] = MAX_RATE * ( 1. - cos((td->t[i] - T0)/DELTA_T1*2*M_PI));
|
||||
else
|
||||
td->rate[i] = MAX_RATE * ( -1. + cos((td->t[i] - T1)/DELTA_T2*2*M_PI));
|
||||
td->angle[i] = td->angle[i-1] + td->rate[i] * td->dt;
|
||||
}
|
||||
td->bias[i] = td->t[i] < 30. ? BIAS_GYRO * ( 1. + td->t[i] * DBIAS_GYRO) :
|
||||
BIAS_GYRO * ( 1. + 30. * DBIAS_GYRO);
|
||||
}
|
||||
|
||||
for (i=0; i< NB_POINTS; i++) {
|
||||
td->gyro[i] = td->rate[i] + td->bias[i] + NOISE_GYRO * ukf_filter_rand_normal();
|
||||
td->ay[i] = G * sin(td->angle[i]) + NOISE_ACCEL * ukf_filter_rand_normal();
|
||||
td->az[i] = G * cos(td->angle[i]) + NOISE_ACCEL * ukf_filter_rand_normal();
|
||||
td->m_angle[i] = atan2(td->ay[i], td->az[i]);
|
||||
}
|
||||
|
||||
return td;
|
||||
}
|
||||
|
||||
void tilt_data_save_state(struct tilt_data* td, int idx, double* X, double* P) {
|
||||
td->est_angle[idx] = X[0];
|
||||
td->est_bias[idx] = X[1];
|
||||
/* FIXME : is this correct ?? not sure about the row/col order */
|
||||
td->P00[idx] = P[0];
|
||||
td->P01[idx] = P[1];
|
||||
td->P10[idx] = P[2];
|
||||
td->P11[idx] = P[3];
|
||||
}
|
||||
|
||||
struct tilt_data* tilt_data_read_log(const char* filename) {
|
||||
GError* _err = NULL;
|
||||
GIOChannel* in_c = g_io_channel_new_file(filename, "r", &_err);
|
||||
if (_err) {
|
||||
g_free(_err);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
GArray* ga_gyro = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
GArray* ga_ay = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
GArray* ga_az = g_array_new(FALSE, FALSE, sizeof(double));
|
||||
|
||||
GString *str = g_string_sized_new(256);
|
||||
GIOStatus st;
|
||||
do {
|
||||
double gx, gy, gz, ax, ay, az;
|
||||
st = g_io_channel_read_line_string(in_c, str, NULL, &_err);
|
||||
if (g_str_has_prefix(str->str, "IMU_GYRO")) {
|
||||
if (sscanf(str->str, "IMU_GYRO %lf %lf %lf", &gx, &gy, &gz) == 3) {
|
||||
g_array_append_val(ga_gyro, gx);
|
||||
}
|
||||
}
|
||||
else if (g_str_has_prefix(str->str, "IMU_ACCEL")) {
|
||||
if (sscanf(str->str, "IMU_ACCEL %lf %lf %lf", &ax, &ay, &az) == 3) {
|
||||
g_array_append_val(ga_ay, ay);
|
||||
g_array_append_val(ga_az, az);
|
||||
}
|
||||
}
|
||||
}
|
||||
while (st != G_IO_STATUS_EOF);
|
||||
g_string_free(str, TRUE);
|
||||
g_free(in_c);
|
||||
|
||||
struct tilt_data* td = tilt_data_new(ga_gyro->len, 0.015625);
|
||||
int i;
|
||||
for (i=0; i<ga_gyro->len; i++) {
|
||||
double* ggx = &g_array_index(ga_gyro, double, i);
|
||||
td->gyro[i] = *ggx;
|
||||
double* gay = &g_array_index(ga_ay, double, i);
|
||||
td->ay[i] = *gay;
|
||||
double* gaz = &g_array_index(ga_az, double, i);
|
||||
td->az[i] = *gaz;
|
||||
td->m_angle[i] = atan2(td->ay[i], td->az[i]);
|
||||
}
|
||||
|
||||
return td;
|
||||
}
|
||||
|
||||
struct tilt_data* tilt_data_new(int len, double dt) {
|
||||
struct tilt_data* td = malloc(sizeof(struct tilt_data));
|
||||
td->dt = dt;
|
||||
td->nb_samples = len;
|
||||
|
||||
td->t = malloc(td->nb_samples*sizeof(double));
|
||||
td->rate = malloc(td->nb_samples*sizeof(double));
|
||||
td->angle = malloc(td->nb_samples*sizeof(double));
|
||||
td->bias = malloc(td->nb_samples*sizeof(double));
|
||||
|
||||
td->gyro = malloc(td->nb_samples*sizeof(double));
|
||||
td->ay = malloc(td->nb_samples*sizeof(double));
|
||||
td->az = malloc(td->nb_samples*sizeof(double));
|
||||
td->m_angle = malloc(td->nb_samples*sizeof(double));
|
||||
|
||||
td->est_angle = malloc(td->nb_samples*sizeof(double));
|
||||
td->est_bias = malloc(td->nb_samples*sizeof(double));
|
||||
|
||||
td->P00 = malloc(td->nb_samples*sizeof(double));
|
||||
td->P01 = malloc(td->nb_samples*sizeof(double));
|
||||
td->P10 = malloc(td->nb_samples*sizeof(double));
|
||||
td->P11 = malloc(td->nb_samples*sizeof(double));
|
||||
|
||||
int i;
|
||||
for (i=0; i< td->nb_samples; i++) {
|
||||
td->t[i] = i * td->dt;
|
||||
td->rate[i] = 0.;
|
||||
td->angle[i] = 0.;
|
||||
td->bias[i] = 0.;
|
||||
|
||||
td->gyro[i] = 0.;
|
||||
td->ay[i] = 0.;
|
||||
td->az[i] = 0.;
|
||||
td->m_angle[i] = 0.;
|
||||
|
||||
td->est_angle[i] = 0.;
|
||||
td->est_bias[i] = 0.;
|
||||
td->P00[i] = 0.;
|
||||
td->P01[i] = 0.;
|
||||
td->P10[i] = 0.;
|
||||
td->P11[i] = 0.;
|
||||
}
|
||||
return td;
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
#ifndef TILT_DATA_H
|
||||
#define TILT_DATA_H
|
||||
|
||||
struct tilt_data {
|
||||
double dt;
|
||||
int nb_samples;
|
||||
double* t;
|
||||
double* rate;
|
||||
double* angle;
|
||||
double* bias;
|
||||
|
||||
double* gyro;
|
||||
double* ay;
|
||||
double* az;
|
||||
double* m_angle;
|
||||
|
||||
double* est_angle;
|
||||
double* est_bias;
|
||||
|
||||
double* P00;
|
||||
double* P01;
|
||||
double* P10;
|
||||
double* P11;
|
||||
|
||||
};
|
||||
|
||||
extern struct tilt_data* tilt_data_gen(void);
|
||||
extern struct tilt_data* tilt_data_read_log(const char* filename);
|
||||
extern struct tilt_data* tilt_data_new(int len, double dt);
|
||||
extern void tilt_data_save_state(struct tilt_data* td, int idx, double* X, double* P);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /* TILT_DATA_H */
|
||||
@@ -0,0 +1,143 @@
|
||||
#include "tilt_display.h"
|
||||
|
||||
#include <gtkdatabox.h>
|
||||
#include <gtkdatabox_points.h>
|
||||
#include <gtkdatabox_lines.h>
|
||||
#include <gtkdatabox_bars.h>
|
||||
#include <gtkdatabox_grid.h>
|
||||
#include <gtkdatabox_cross_simple.h>
|
||||
#include <gtkdatabox_marker.h>
|
||||
|
||||
GtkWidget* tilt_display(struct tilt_data* td) {
|
||||
GtkWidget* window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
|
||||
gtk_widget_set_size_request (window, 1280, 800);
|
||||
|
||||
GtkWidget* vbox1 = gtk_vbox_new (FALSE, 0);
|
||||
gtk_container_add (GTK_CONTAINER (window), vbox1);
|
||||
|
||||
GtkWidget *frame = gtk_frame_new ("rate");
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
|
||||
GtkWidget* databox = gtk_databox_new();
|
||||
gfloat* prate = g_new0 (gfloat, td->nb_samples);
|
||||
gfloat* p_m_rate = g_new0 (gfloat, td->nb_samples);
|
||||
gfloat* pt = g_new0 (gfloat, td->nb_samples);
|
||||
int idx;
|
||||
for (idx=0; idx<td->nb_samples; idx++) {
|
||||
prate[idx] = td->rate[idx];
|
||||
p_m_rate[idx] = td->gyro[idx];
|
||||
pt[idx] = td->t[idx];
|
||||
}
|
||||
GdkColor red = {0, 65535, 0, 0};
|
||||
GdkColor green = {0, 0, 65535, 0};
|
||||
GdkColor blue = {0, 0, 0, 65535};
|
||||
GdkColor pink = {0, 65535, 0, 65535};
|
||||
GdkColor black = {0, 0, 0, 0};
|
||||
|
||||
GtkDataboxGraph *grate = gtk_databox_lines_new (td->nb_samples, pt, prate, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), grate);
|
||||
|
||||
GtkDataboxGraph *g_m_rate = gtk_databox_lines_new (td->nb_samples, pt, p_m_rate, &green, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), g_m_rate);
|
||||
|
||||
GtkDataboxGraph *grid = gtk_databox_grid_new (10, 10, &black, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), grid);
|
||||
|
||||
gtk_container_add (GTK_CONTAINER (frame), databox );
|
||||
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
|
||||
|
||||
|
||||
|
||||
frame = gtk_frame_new ("angle");
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
|
||||
databox = gtk_databox_new();
|
||||
gfloat* pangle = g_new0 (gfloat, td->nb_samples);
|
||||
gfloat* pm_angle = g_new0 (gfloat, td->nb_samples);
|
||||
gfloat* pe_angle = g_new0 (gfloat, td->nb_samples);
|
||||
for (idx=0; idx<td->nb_samples; idx++) {
|
||||
pangle[idx] = td->angle[idx];
|
||||
pm_angle[idx] = td->m_angle[idx];
|
||||
pe_angle[idx] = td->est_angle[idx];
|
||||
}
|
||||
|
||||
GtkDataboxGraph *gangle = gtk_databox_lines_new (td->nb_samples, pt, pangle, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), gangle);
|
||||
|
||||
GtkDataboxGraph *ge_angle = gtk_databox_lines_new (td->nb_samples, pt, pe_angle, &blue, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), ge_angle);
|
||||
|
||||
GtkDataboxGraph *gm_angle = gtk_databox_lines_new (td->nb_samples, pt, pm_angle, &green, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), gm_angle);
|
||||
|
||||
grid = gtk_databox_grid_new (10, 10, &black, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), grid);
|
||||
|
||||
gtk_container_add (GTK_CONTAINER (frame), databox );
|
||||
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
|
||||
|
||||
|
||||
frame = gtk_frame_new ("bias");
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
|
||||
databox = gtk_databox_new();
|
||||
gfloat* pbias = g_new0 (gfloat, td->nb_samples);
|
||||
gfloat* pest_bias = g_new0 (gfloat, td->nb_samples);
|
||||
for (idx=0; idx<td->nb_samples; idx++) {
|
||||
pbias[idx] = td->bias[idx];
|
||||
pest_bias[idx] = td->est_bias[idx];
|
||||
}
|
||||
GtkDataboxGraph *gest_bias = gtk_databox_lines_new (td->nb_samples, pt, pest_bias, &blue, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), gest_bias);
|
||||
|
||||
GtkDataboxGraph *gbias = gtk_databox_lines_new (td->nb_samples, pt, pbias, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), gbias);
|
||||
|
||||
grid = gtk_databox_grid_new (10, 10, &black, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), grid);
|
||||
|
||||
gtk_container_add (GTK_CONTAINER (frame), databox );
|
||||
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
|
||||
|
||||
|
||||
frame = gtk_frame_new ("covariance");
|
||||
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
|
||||
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
|
||||
|
||||
databox = gtk_databox_new();
|
||||
gfloat* pP00 = g_new0 (gfloat, td->nb_samples);
|
||||
gfloat* pP01 = g_new0 (gfloat, td->nb_samples);
|
||||
gfloat* pP10 = g_new0 (gfloat, td->nb_samples);
|
||||
gfloat* pP11 = g_new0 (gfloat, td->nb_samples);
|
||||
for (idx=0; idx<td->nb_samples; idx++) {
|
||||
pP00[idx] = td->P00[idx];
|
||||
pP01[idx] = td->P01[idx];
|
||||
pP10[idx] = td->P10[idx];
|
||||
pP11[idx] = td->P11[idx];
|
||||
}
|
||||
GtkDataboxGraph *gP00 = gtk_databox_lines_new (td->nb_samples, pt, pP00, &red, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), gP00);
|
||||
|
||||
GtkDataboxGraph *gP01 = gtk_databox_lines_new (td->nb_samples, pt, pP01, &green, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), gP01);
|
||||
|
||||
GtkDataboxGraph *gP10 = gtk_databox_lines_new (td->nb_samples, pt, pP10, &blue, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), gP10);
|
||||
|
||||
GtkDataboxGraph *gP11 = gtk_databox_lines_new (td->nb_samples, pt, pP11, &pink, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), gP11);
|
||||
|
||||
grid = gtk_databox_grid_new (10, 10, &black, 1);
|
||||
gtk_databox_graph_add (GTK_DATABOX(databox), grid);
|
||||
|
||||
gtk_container_add (GTK_CONTAINER (frame), databox );
|
||||
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
|
||||
|
||||
|
||||
gtk_widget_show_all(window);
|
||||
return window;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
#include <gtk/gtk.h>
|
||||
|
||||
#include "tilt_data.h"
|
||||
|
||||
extern GtkWidget* tilt_display(struct tilt_data* td);
|
||||
@@ -0,0 +1,94 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "tilt_data.h"
|
||||
#include "tilt_display.h"
|
||||
#include "tilt_utils.h"
|
||||
#include "ekf.h"
|
||||
|
||||
/*
|
||||
Simple 2 state filter for hybridizing gyrometer and accelerometer
|
||||
on one axis.
|
||||
The filter wil track the gyro bias.
|
||||
|
||||
state : [ angle gyro_bias ]
|
||||
|
||||
*/
|
||||
|
||||
static struct tilt_data* td;
|
||||
static int iter;
|
||||
|
||||
void linear_filter(double *u, double* x, double* dt, double *Xdot, double* F) {
|
||||
/* state prediction
|
||||
angle += angle_dot * dt
|
||||
bias += 0
|
||||
*/
|
||||
*dt = td->dt;
|
||||
Xdot[0] = (u[0] - x[1]);
|
||||
Xdot[1] = 0.;
|
||||
/* Jacobian of xdot wrt the state
|
||||
F = [ d(angle_dot)/d(angle) d(angle_dot)/d(gyro_bias) ]
|
||||
[ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]
|
||||
*/
|
||||
F[0] = 0.;
|
||||
F[1] = -1.;
|
||||
F[2] = 0.;
|
||||
F[3] = 0.;
|
||||
}
|
||||
|
||||
void linear_measure(double *y, double* err, double*x, double *H) {
|
||||
/* err = measure - estimate */
|
||||
*err = y[0] - x[0];
|
||||
/* H is the jacobian of the measure wrt X */
|
||||
H[0] = 1.;
|
||||
H[1] = 0.;
|
||||
}
|
||||
|
||||
|
||||
void run_ekf(struct tilt_data* td) {
|
||||
/* model noise covariance matrix */
|
||||
double Q[4]={0.001, 0.0,
|
||||
0.0, 0.003};
|
||||
/* measurement noise covariance matrix */
|
||||
double R[1]={1.7};
|
||||
/* initial x */
|
||||
double X0[2] = {0.0, 0.0};
|
||||
/* initial state covariance matrix */
|
||||
double P0[4] = {1.0, 0.0,
|
||||
0.0, 1.0};
|
||||
/* measure */
|
||||
double y[1];
|
||||
/* command */
|
||||
double u[1];
|
||||
|
||||
struct ekf_filter* filter;
|
||||
filter = ekf_filter_new(2, 1, Q, R, linear_filter, linear_measure);
|
||||
tilt_init(td, 150, X0);
|
||||
ekf_filter_reset(filter, X0, P0);
|
||||
|
||||
/* filter run */
|
||||
for (iter=0; iter<td->nb_samples; iter++) {
|
||||
u[0] = td->gyro[iter];
|
||||
y[0] = td->m_angle[iter];
|
||||
ekf_filter_predict(filter, u);
|
||||
ekf_filter_update(filter, y);
|
||||
ekf_filter_get_state(filter, X0, P0);
|
||||
tilt_data_save_state(td, iter, X0, P0);
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
main(int argc, char *argv[]) {
|
||||
gtk_init(&argc, &argv);
|
||||
|
||||
//td = tilt_data_gen();
|
||||
//td = tilt_data_read_log("data/log_ahrs_still");
|
||||
td = tilt_data_read_log("../data/log_ahrs_roll");
|
||||
//td = tilt_data_read_log("data/log_ahrs_bug");
|
||||
//td = tilt_data_read_log("data/log_ahrs_yaw_pitched");
|
||||
run_ekf(td);
|
||||
tilt_display(td);
|
||||
|
||||
gtk_main();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "tilt_data.h"
|
||||
#include "tilt_display.h"
|
||||
#include "tilt_utils.h"
|
||||
|
||||
static struct tilt_data* td;
|
||||
|
||||
|
||||
#define CONTINUOUS
|
||||
|
||||
void run_tilt(void) {
|
||||
/* state */
|
||||
double X[2];
|
||||
/* state covariance matrix */
|
||||
double P[4] = { 1., 0.,
|
||||
0., 1. };
|
||||
/* model noise covariance matrix */
|
||||
double Q[4]={0.001, 0.0,
|
||||
0.0, 0.003 };
|
||||
/* jacobian of the measure wrt X */
|
||||
const double H[2] = { 1., 0. };
|
||||
/* measurement covariance noise */
|
||||
const double R = 0.5;
|
||||
|
||||
tilt_init(td, 150, X);
|
||||
|
||||
int iter;
|
||||
for (iter=0; iter<td->nb_samples; iter++) {
|
||||
|
||||
double rate = td->gyro[iter] - X[1];
|
||||
/* X += Xdot * dt */
|
||||
X[0] += rate * td->dt;
|
||||
|
||||
#ifdef EKF_UPDATE_CONTINUOUS
|
||||
/* Pdot = F*P + P*F' + Q
|
||||
* F = { 1, 0,
|
||||
* 0, 0 };
|
||||
*/
|
||||
double Pdot[4] = { Q[0] - P[0*2 + 1] - P[1*2 + 0], -P[1*2 + 1],
|
||||
-P[1*2 + 1] , Q[1*2 + 1] };
|
||||
#endif
|
||||
#ifdef EKF_UPDATE_DISCRETE
|
||||
/* Pdot = F*P*F' + Q */
|
||||
double Pdot[4] = { P[1*2 + 1] +Q[0*2 + 0], 0.,
|
||||
0. , Q[1*2 + 1] };
|
||||
#endif
|
||||
/* P += Pdot * dt */
|
||||
P[0*2 + 0] += Pdot[0*2 + 0] * td->dt;
|
||||
P[0*2 + 1] += Pdot[0*2 + 1] * td->dt;
|
||||
P[1*2 + 0] += Pdot[1*2 + 0] * td->dt;
|
||||
P[1*2 + 1] += Pdot[1*2 + 1] * td->dt;
|
||||
|
||||
/* E = H * P * H' + R */
|
||||
const double PHt_0 = P[0*2 + 0] * H[0]; // + P[0*2 + 1] * H[1]
|
||||
const double PHt_1 = P[1*2 + 0] * H[0]; // + P[1*2 + 1] * H[1]
|
||||
const double E = H[0] * PHt_0 + // H[1] * PHt1
|
||||
R;
|
||||
|
||||
/* K = P * H' * inv(E) */
|
||||
const double K_0 = PHt_0 / E;
|
||||
const double K_1 = PHt_1 / E;
|
||||
|
||||
/* P = P - K * H * P */
|
||||
const double t_0 = PHt_0; /* H[0] * P[0][0] + H[1] * P[1][0] */
|
||||
const double t_1 = H[0] * P[0*2+ 1]; /* + H[1] * P[1][1] = 0 */
|
||||
P[0*2 + 0] -= K_0 * t_0;
|
||||
P[0*2 + 1] -= K_0 * t_1;
|
||||
P[1*2 + 0] -= K_1 * t_0;
|
||||
P[1*2 + 1] -= K_1 * t_1;
|
||||
|
||||
/* X += K * err */
|
||||
double err = td->m_angle[iter] - X[0];
|
||||
X[0] += K_0 * err;
|
||||
X[1] += K_1 * err;
|
||||
|
||||
tilt_data_save_state(td, iter, X, P);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
gtk_init(&argc, &argv);
|
||||
|
||||
//td = tilt_data_gen();
|
||||
//td = tilt_data_read_log("data/log_ahrs_still");
|
||||
//td = tilt_data_read_log("data/log_ahrs_roll");
|
||||
td = tilt_data_read_log("data/log_ahrs_bug");
|
||||
//td = tilt_data_read_log("data/log_ahrs_yaw_pitched");
|
||||
|
||||
run_tilt();
|
||||
|
||||
tilt_display(td);
|
||||
|
||||
gtk_main();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "tilt_data.h"
|
||||
#include "tilt_display.h"
|
||||
#include "tilt_utils.h"
|
||||
|
||||
static struct tilt_data* td;
|
||||
|
||||
|
||||
#define CONTINUOUS
|
||||
|
||||
void run_tilt(void) {
|
||||
/* state */
|
||||
double X[2];
|
||||
/* state covariance matrix */
|
||||
double P[4] = { 1., 0.,
|
||||
0., 1. };
|
||||
/* model noise covariance matrix */
|
||||
double Q[4]={0.001, 0.0,
|
||||
0.0, 0.003 };
|
||||
/* jacobian of the measure wrt X */
|
||||
const double H[2] = { 1., 0. };
|
||||
/* measurement covariance noise */
|
||||
const double R = 0.5;
|
||||
|
||||
tilt_init(td, 150, X);
|
||||
|
||||
int iter;
|
||||
for (iter=0; iter<td->nb_samples; iter++) {
|
||||
|
||||
double rate = td->gyro[iter] - X[1];
|
||||
/* X += Xdot * dt */
|
||||
X[0] += rate * td->dt;
|
||||
|
||||
#ifdef EKF_UPDATE_CONTINUOUS
|
||||
/* Pdot = F*P + P*F' + Q
|
||||
* F = { 1, 0,
|
||||
* 0, 0 };
|
||||
*/
|
||||
double Pdot[4] = { Q[0] - P[0*2 + 1] - P[1*2 + 0], -P[1*2 + 1],
|
||||
-P[1*2 + 1] , Q[1*2 + 1] };
|
||||
#endif
|
||||
#ifdef EKF_UPDATE_DISCRETE
|
||||
/* Pdot = F*P*F' + Q */
|
||||
double Pdot[4] = { P[1*2 + 1] +Q[0*2 + 0], 0.,
|
||||
0. , Q[1*2 + 1] };
|
||||
#endif
|
||||
/* P += Pdot * dt */
|
||||
P[0*2 + 0] += Pdot[0*2 + 0] * td->dt;
|
||||
P[0*2 + 1] += Pdot[0*2 + 1] * td->dt;
|
||||
P[1*2 + 0] += Pdot[1*2 + 0] * td->dt;
|
||||
P[1*2 + 1] += Pdot[1*2 + 1] * td->dt;
|
||||
|
||||
/* E = H * P * H' + R */
|
||||
const double PHt_0 = P[0*2 + 0] * H[0]; // + P[0*2 + 1] * H[1]
|
||||
const double PHt_1 = P[1*2 + 0] * H[0]; // + P[1*2 + 1] * H[1]
|
||||
const double E = H[0] * PHt_0 + // H[1] * PHt1
|
||||
R;
|
||||
|
||||
/* K = P * H' * inv(E) */
|
||||
const double K_0 = PHt_0 / E;
|
||||
const double K_1 = PHt_1 / E;
|
||||
|
||||
/* P = P - K * H * P */
|
||||
const double t_0 = PHt_0; /* H[0] * P[0][0] + H[1] * P[1][0] */
|
||||
const double t_1 = H[0] * P[0*2+ 1]; /* + H[1] * P[1][1] = 0 */
|
||||
P[0*2 + 0] -= K_0 * t_0;
|
||||
P[0*2 + 1] -= K_0 * t_1;
|
||||
P[1*2 + 0] -= K_1 * t_0;
|
||||
P[1*2 + 1] -= K_1 * t_1;
|
||||
|
||||
/* X += K * err */
|
||||
double err = td->m_angle[iter] - X[0];
|
||||
X[0] += K_0 * err;
|
||||
X[1] += K_1 * err;
|
||||
|
||||
tilt_data_save_state(td, iter, X, P);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
gtk_init(&argc, &argv);
|
||||
|
||||
//td = tilt_data_gen();
|
||||
//td = tilt_data_read_log("../data/log_ahrs_still");
|
||||
//td = tilt_data_read_log("../data/log_ahrs_roll");
|
||||
td = tilt_data_read_log("../data/log_ahrs_bug");
|
||||
//td = tilt_data_read_log("../data/log_ahrs_yaw_pitched");
|
||||
|
||||
run_tilt();
|
||||
|
||||
tilt_display(td);
|
||||
|
||||
gtk_main();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "tilt_data.h"
|
||||
#include "tilt_display.h"
|
||||
#include "tilt_utils.h"
|
||||
#include "ukf.h"
|
||||
|
||||
/*
|
||||
Simple 2 state filter for hybridizing gyrometer and accelerometer
|
||||
on one axis.
|
||||
The filter wil track the gyro bias.
|
||||
|
||||
state : [ angle gyro_bias ]
|
||||
|
||||
*/
|
||||
static struct tilt_data* td;
|
||||
|
||||
/* x1 = A.x0 + B + u */
|
||||
void linear_filter(double *x1, double *x0, double *u) {
|
||||
// printf("in linear_filter (%d)\n", iter);
|
||||
x1[0] = x0[0] + (u[0] - x0[1])*td->dt;
|
||||
x1[1] = x0[1];
|
||||
|
||||
}
|
||||
|
||||
/* y = H.x + C */
|
||||
void linear_measure(double *y, double *x) {
|
||||
// printf("in linear_measure (%d)\n", iter);
|
||||
y[0] = x[0];
|
||||
}
|
||||
|
||||
|
||||
void run_ukf(struct tilt_data* td) {
|
||||
/* model noise covariance matrix */
|
||||
double Q[4]={0.001, 0.0,
|
||||
0.0, 0.003 };
|
||||
/* measurement noise covariance matrix */
|
||||
double R[1]={0.5};
|
||||
/* initial x */
|
||||
double x[2] = {0.0, 0.0};
|
||||
/* initial state covariance matrix */
|
||||
double P[4] = {1.0, 0.0,
|
||||
0.0, 1.0};
|
||||
/* measure */
|
||||
double y[1];
|
||||
/* command */
|
||||
double u[2] = {0.0, 0.0};
|
||||
|
||||
ukf_filter filter;
|
||||
filter = ukf_filter_new(2, 1, Q, R, linear_filter, linear_measure);
|
||||
tilt_init(td, 150, x);
|
||||
ukf_filter_reset(filter, x, P);
|
||||
ukf_filter_compute_weights(filter, 1.1, 0.0, 2.0);
|
||||
|
||||
/* filter run */
|
||||
int iter;
|
||||
for (iter=0; iter<td->nb_samples; iter++) {
|
||||
u[0] = td->gyro[iter];
|
||||
y[0] = td->m_angle[iter];
|
||||
ukf_filter_update(filter, y, u);
|
||||
ukf_filter_get_state(filter, x, P);
|
||||
tilt_data_save_state(td, iter, x, P);
|
||||
}
|
||||
ukf_filter_delete(filter);
|
||||
}
|
||||
|
||||
int
|
||||
main(int argc, char *argv[]) {
|
||||
gtk_init(&argc, &argv);
|
||||
|
||||
//td = tilt_data_gen();
|
||||
//td = tilt_data_read_log("../data/log_ahrs_still");
|
||||
td = tilt_data_read_log("../data/log_ahrs_roll");
|
||||
//td = tilt_data_read_log("../data/log_ahrs_bug");
|
||||
//td = tilt_data_read_log("../data/log_ahrs_yaw_pitched");
|
||||
run_ukf(td);
|
||||
tilt_display(td);
|
||||
|
||||
gtk_main();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
#include "tilt_utils.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
void tilt_init(struct tilt_data* td, int len, double* X) {
|
||||
/* initialisation : assume vehicle is still */
|
||||
int iter;
|
||||
X[0] = 0.;
|
||||
X[1] = 0.;
|
||||
for (iter=0; iter < len; iter++) {
|
||||
X[0] += td->m_angle[iter];
|
||||
X[1] += td->gyro[iter];
|
||||
}
|
||||
X[0] /= (double)len;
|
||||
X[1] /= (double)len;
|
||||
printf("initialisation angle %f bias %f\n", X[0], X[1]);
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
#ifndef TILT_UTILS_H
|
||||
#define TILT_UTILS_H
|
||||
|
||||
#include "tilt_data.h"
|
||||
|
||||
extern void tilt_init( struct tilt_data* td, int len, double* X);
|
||||
|
||||
|
||||
#endif /* TILT_UTILS_H */
|
||||
@@ -0,0 +1,453 @@
|
||||
#include"ukf.h"
|
||||
#include"linalg.h"
|
||||
#include<math.h>
|
||||
#include<string.h>
|
||||
#include<stdlib.h>
|
||||
#include<stdio.h>
|
||||
|
||||
struct ukf_filter_t {
|
||||
// filter state
|
||||
unsigned state_dim;
|
||||
unsigned measure_dim;
|
||||
double *x;
|
||||
// measure
|
||||
double *y;
|
||||
// matrice de covariance de l'état
|
||||
double *P;
|
||||
// matrice de covariance du bruit de modèle (additif)
|
||||
double *Q;
|
||||
// matrice de covariance du bruit de mesure (additif)
|
||||
double *R;
|
||||
// fonction d'évolution
|
||||
filter_function ffun;
|
||||
// fonction de mesure
|
||||
measure_function mfun;
|
||||
// all fields below are specific to UKF
|
||||
// weights for computing the mean
|
||||
double *wm;
|
||||
// weights for computing the covariance
|
||||
double *wc;
|
||||
// scaling parameter
|
||||
double gamma;
|
||||
// sigma points (first one is filter state)
|
||||
double *sigma_point;
|
||||
// preallocated temporaries
|
||||
double *sigma;
|
||||
double *sigma_y;
|
||||
double *PM;
|
||||
double *PM_save;
|
||||
double *xm;
|
||||
double *ym;
|
||||
double *khi;
|
||||
double *khi_y;
|
||||
double *Pyy;
|
||||
double *Pxy;
|
||||
double *dx;
|
||||
double *dy;
|
||||
double *gain;
|
||||
double *KL;
|
||||
};
|
||||
|
||||
void
|
||||
ukf_safe_free(void *p) {
|
||||
if(p != 0) free(p);
|
||||
}
|
||||
|
||||
/*
|
||||
* ukf_filter_new
|
||||
*/
|
||||
|
||||
ukf_filter
|
||||
ukf_filter_new(unsigned state_dim,
|
||||
unsigned measure_dim,
|
||||
double *Q,
|
||||
double *R,
|
||||
filter_function ffun,
|
||||
measure_function mfun) {
|
||||
ukf_filter filter;
|
||||
int n;
|
||||
unsigned err = 0;
|
||||
// nothing to do if no state or measurement !
|
||||
if(state_dim == 0 || measure_dim == 0)
|
||||
return 0;
|
||||
// alloc new structure
|
||||
filter = malloc(sizeof(struct ukf_filter_t));
|
||||
// returns 0 if allocation fails
|
||||
if(filter == 0)
|
||||
return 0;
|
||||
// fills the structure
|
||||
filter->state_dim = state_dim;
|
||||
filter->measure_dim = measure_dim;
|
||||
filter->ffun = ffun;
|
||||
filter->mfun = mfun;
|
||||
|
||||
filter->x = malloc(state_dim * sizeof(double));
|
||||
err |= (filter->x == 0);
|
||||
|
||||
filter->y = malloc(measure_dim * sizeof(double));
|
||||
err |= (filter->y == 0);
|
||||
|
||||
n = state_dim * state_dim;
|
||||
|
||||
filter->P = malloc(n * sizeof(double));
|
||||
err |= (filter->P == 0);
|
||||
|
||||
n = 2 * state_dim + 1;
|
||||
|
||||
filter->wm = malloc(n * sizeof(double));
|
||||
err |= (filter->wm == 0);
|
||||
|
||||
filter->wc = malloc(n * sizeof(double));
|
||||
err |= (filter->wc == 0);
|
||||
|
||||
filter->sigma_point = malloc(n * state_dim * sizeof(double));
|
||||
err |= (filter->sigma_point == 0);
|
||||
|
||||
n = filter->state_dim;
|
||||
|
||||
filter->sigma = malloc(n * sizeof(double));
|
||||
err |= (filter->sigma == 0);
|
||||
|
||||
filter->PM = malloc(n * n * sizeof(double));
|
||||
err |= (filter->PM == 0);
|
||||
|
||||
filter->PM_save = malloc(n * n * sizeof(double));
|
||||
err |= (filter->PM == 0);
|
||||
|
||||
filter->xm = malloc(n * sizeof(double));
|
||||
err |= (filter->xm == 0);
|
||||
|
||||
filter->ym = malloc(filter->measure_dim * sizeof(double));
|
||||
err |= (filter->ym == 0);
|
||||
|
||||
n = 2 * filter->state_dim + 1;
|
||||
filter->khi = malloc(n * filter->state_dim * sizeof(double));
|
||||
err |= (filter->khi == 0);
|
||||
|
||||
filter->khi_y = malloc(n * filter->measure_dim * sizeof(double));
|
||||
err |= (filter->khi_y == 0);
|
||||
|
||||
n = filter->measure_dim;
|
||||
filter->Pyy = malloc(n * n * sizeof(double));
|
||||
err |= (filter->Pyy == 0);
|
||||
|
||||
filter->Pxy = malloc(n * filter->state_dim * sizeof(double));
|
||||
err |= (filter->Pxy == 0);
|
||||
|
||||
|
||||
filter->dx = malloc(filter->state_dim * sizeof(double));
|
||||
err |= (filter->dx == 0);
|
||||
|
||||
filter->dy = malloc(filter->measure_dim * sizeof(double));
|
||||
err |= (filter->dy == 0);
|
||||
|
||||
filter->gain = malloc(filter->state_dim * filter->measure_dim * sizeof(double));
|
||||
err |= (filter->gain == 0);
|
||||
|
||||
filter->sigma_y = malloc(filter->measure_dim * sizeof(double));
|
||||
err |= (filter->sigma_y == 0);
|
||||
|
||||
filter->KL = malloc(filter->state_dim * filter->measure_dim * sizeof(double));
|
||||
err |= (filter->KL == 0);
|
||||
|
||||
if(err != 0) {
|
||||
ukf_filter_delete(filter);
|
||||
return 0;
|
||||
}
|
||||
|
||||
n = filter->state_dim;
|
||||
filter->Q = malloc(n * n * sizeof(double));
|
||||
if(filter->Q == 0) {
|
||||
ukf_filter_delete(filter);
|
||||
return 0;
|
||||
}
|
||||
memcpy(filter->Q, Q, n * n * sizeof(double));
|
||||
|
||||
n = filter->measure_dim;
|
||||
filter->R = malloc(n * n * sizeof(double));
|
||||
if(filter->R == 0) {
|
||||
ukf_filter_delete(filter);
|
||||
return 0;
|
||||
}
|
||||
memcpy(filter->R, R, n * n * sizeof(double));
|
||||
|
||||
// returns the newly allocated structure
|
||||
return filter;
|
||||
}
|
||||
|
||||
/*
|
||||
* ukf_filter_delete
|
||||
*/
|
||||
|
||||
void
|
||||
ukf_filter_delete(ukf_filter filter) {
|
||||
if(filter != 0) {
|
||||
ukf_safe_free(filter->x);
|
||||
ukf_safe_free(filter->y);
|
||||
ukf_safe_free(filter->P);
|
||||
ukf_safe_free(filter->Q);
|
||||
ukf_safe_free(filter->R);
|
||||
ukf_safe_free(filter->wm);
|
||||
ukf_safe_free(filter->wc);
|
||||
ukf_safe_free(filter->sigma_point);
|
||||
ukf_safe_free(filter->sigma);
|
||||
ukf_safe_free(filter->PM);
|
||||
ukf_safe_free(filter->PM_save);
|
||||
ukf_safe_free(filter->xm);
|
||||
ukf_safe_free(filter->ym);
|
||||
ukf_safe_free(filter->khi);
|
||||
ukf_safe_free(filter->khi_y);
|
||||
ukf_safe_free(filter->Pyy);
|
||||
ukf_safe_free(filter->Pxy);
|
||||
ukf_safe_free(filter->dx);
|
||||
ukf_safe_free(filter->dy);
|
||||
ukf_safe_free(filter->gain);
|
||||
ukf_safe_free(filter->sigma_y);
|
||||
ukf_safe_free(filter->KL);
|
||||
free(filter);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* ukf_filter_compute_weights
|
||||
*/
|
||||
|
||||
void
|
||||
ukf_filter_compute_weights(ukf_filter filter,
|
||||
double alpha,
|
||||
double k,
|
||||
double beta) {
|
||||
double l;
|
||||
double lam;
|
||||
unsigned i;
|
||||
|
||||
if(filter == 0) return;
|
||||
l = (double)filter->state_dim;
|
||||
// lambda parameter
|
||||
lam = alpha * alpha *( l + k) - l;
|
||||
|
||||
filter->wm[0] = lam / (lam + l);
|
||||
filter->wc[0] = filter->wm[0] + (1.0 - alpha * alpha + beta);
|
||||
for(i = 1 ; i <= 2*filter->state_dim ; i++) {
|
||||
filter->wm[i] = 0.5 / (lam + l);
|
||||
filter->wc[i] = 0.5 / (lam + l);
|
||||
}
|
||||
filter->gamma = alpha*sqrt(l + k);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ukf_filter_reset
|
||||
*/
|
||||
|
||||
void
|
||||
ukf_filter_reset(ukf_filter filter,
|
||||
double *x0,
|
||||
double *P0) {
|
||||
if(filter != 0) {
|
||||
// state of the filter
|
||||
memcpy(filter->x, x0, filter->state_dim * sizeof(double));
|
||||
memcpy(filter->P, P0,
|
||||
filter->state_dim * filter->state_dim * sizeof(double));
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* ukf_filter_get_state
|
||||
*/
|
||||
|
||||
void
|
||||
ukf_filter_get_state(ukf_filter filter, double *x, double* P){
|
||||
if(filter != 0) {
|
||||
memcpy(x, filter->x, filter->state_dim * sizeof(double));
|
||||
memcpy(P, filter->P, filter->state_dim * filter->state_dim * sizeof(double));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* ukf_filter_update
|
||||
*/
|
||||
|
||||
void
|
||||
ukf_filter_update(ukf_filter filter, double *y, double *u) {
|
||||
int l = filter->state_dim;
|
||||
int m = filter->measure_dim;
|
||||
int i,j,k;
|
||||
double t;
|
||||
|
||||
// cholesky decomposition of the state covariance matrix
|
||||
|
||||
ukf_cholesky_decomposition(filter->P, l, filter->sigma);
|
||||
|
||||
//=================================
|
||||
// compute sigma points
|
||||
for(j = 0 ; j < l ; j++)
|
||||
filter->sigma_point[j]= filter->x[j];
|
||||
for(i = 0 ; i < l ; i++) {
|
||||
for(j = 0 ; j < i ; j++) {
|
||||
filter->sigma_point[(i + 1) * l + j] = filter->x[j] ;
|
||||
filter->sigma_point[(i + 1 + l) * l + j] = filter->x[j] ;
|
||||
}
|
||||
filter->sigma_point[(i + 1) * l + i] = filter->x[i] + filter->gamma * filter->sigma[i];
|
||||
filter->sigma_point[(i + 1 + l) * l + i] = filter->x[i] - filter->gamma * filter->sigma[i];
|
||||
for(j = i + 1 ; j < l ; j++) {
|
||||
filter->sigma_point[(i + 1) * l + j] = filter->x[j] + filter->gamma * filter->P[j * l + i];
|
||||
filter->sigma_point[(i + 1 + l) * l + j] = filter->x[j] - filter->gamma * filter->P[j * l + i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//=================================
|
||||
// propagate sigma points
|
||||
for(i = 0 ; i < 2 * l + 1 ; i++) {
|
||||
filter->ffun(&(filter->khi[i * l]) , &(filter->sigma_point[i * l]) , u);
|
||||
}
|
||||
|
||||
// compute state prediction xm
|
||||
for(i = 0 ; i < l ; i++) {
|
||||
filter->xm[i] = filter->wm[0] * filter->khi[i];
|
||||
for(j = 1 ; j < 2 * l + 1 ; j++) {
|
||||
filter->xm[i] += filter->wm[j] * filter->khi[j * l + i];
|
||||
}
|
||||
}
|
||||
|
||||
//================================
|
||||
// time update
|
||||
|
||||
// start with state covariance matrix
|
||||
for(i = 0 ; i < l * l ; i++)
|
||||
filter->PM[i] = filter->Q[i];
|
||||
|
||||
// accumulate covariances
|
||||
for(i = 0 ; i < 2 * l + 1 ; i++) {
|
||||
for(j = 0 ; j < l ; j++)
|
||||
filter->dx[j]= filter->khi[i * l + j] - filter->xm[j];
|
||||
for(j = 0 ; j < l ; j++) {
|
||||
for(k = 0 ; k < l ; k++) {
|
||||
filter->PM[j * l + k] += filter->wc[i] * filter->dx[j] * filter->dx[k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// save PM matrix
|
||||
|
||||
for(i = 0 ; i < l * l ; i++)
|
||||
filter->PM_save[i] = filter->PM[i];
|
||||
|
||||
// redraw sigma points
|
||||
|
||||
ukf_cholesky_decomposition(filter->PM, l, filter->sigma);
|
||||
|
||||
for(j = 0 ; j < l ; j++)
|
||||
filter->sigma_point[j]= filter->xm[j];
|
||||
for(i = 0 ; i < l ; i++) {
|
||||
for(j = 0 ; j < i ; j++) {
|
||||
filter->sigma_point[(i + 1) * l + j] = filter->xm[j] ;
|
||||
filter->sigma_point[(i + 1 + l) * l + j] = filter->xm[j] ;
|
||||
}
|
||||
filter->sigma_point[(i + 1) * l + i] = filter->xm[i] + filter->gamma * filter->sigma[i];
|
||||
filter->sigma_point[(i + 1 + l) * l + i] = filter->xm[i] - filter->gamma * filter->sigma[i];
|
||||
for(j = i + 1 ; j < l ; j++) {
|
||||
filter->sigma_point[(i + 1) * l + j] = filter->xm[j] + filter->gamma * filter->PM[j * l + i];
|
||||
filter->sigma_point[(i + 1 + l) * l + j] = filter->xm[j] - filter->gamma * filter->PM[j * l + i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
//=================================
|
||||
// propagate measurement
|
||||
|
||||
for(i = 0 ; i < 2 * l + 1 ; i++) {
|
||||
filter->mfun(&(filter->khi_y[i * m]) , &(filter->sigma_point[i * l]) );
|
||||
}
|
||||
|
||||
// measurement prediction
|
||||
|
||||
for(i = 0 ; i < m ; i++) {
|
||||
filter->ym[i] = filter->wm[0] * filter->khi_y[i];
|
||||
for(j = 1 ; j < 2 * l + 1 ; j++)
|
||||
filter->ym[i] += filter->wm[j] * filter->khi_y[j * m + i];
|
||||
}
|
||||
|
||||
// measurement update
|
||||
|
||||
// Pyy matrix
|
||||
// start with measure covariance matrix
|
||||
|
||||
for(i = 0 ; i < m * m ; i++)
|
||||
filter->Pyy[i] = filter->R[i];
|
||||
|
||||
// accumulate covariances
|
||||
|
||||
for(i = 0 ; i < 2 * l + 1 ; i++) {
|
||||
for(j = 0 ; j < m ; j++)
|
||||
filter->dy[j]= filter->khi_y[i * m + j] - filter->ym[j];
|
||||
for(j = 0 ; j < m ; j++)
|
||||
for(k = 0 ; k < m ; k++) {
|
||||
filter->Pyy[j * m + k] += filter->wc[i] * filter->dy[j] * filter->dy[k];
|
||||
}
|
||||
}
|
||||
|
||||
// Pxy matrix
|
||||
|
||||
for(i = 0 ; i < m * l ; i++)
|
||||
filter->Pxy[i] = 0.0;
|
||||
|
||||
// accumulate covariances
|
||||
|
||||
for(i = 0 ; i < 2 * l + 1 ; i++) {
|
||||
for(j = 0 ; j < m ; j++) {
|
||||
filter->dy[j]= filter->khi_y[i * m + j] - filter->ym[j];
|
||||
}
|
||||
for(j = 0 ; j < l ; j++) {
|
||||
filter->dx[j] = filter->sigma_point[i * l + j] - filter->xm[j];
|
||||
}
|
||||
for(j = 0 ; j < l ; j++)
|
||||
for(k = 0 ; k < m ; k++) {
|
||||
filter->Pxy[j * m + k] += filter->wc[i] * filter->dx[j] * filter->dy[k];
|
||||
}
|
||||
}
|
||||
|
||||
// gain de kalman
|
||||
|
||||
ukf_cholesky_decomposition(filter->Pyy, m, filter->sigma_y);
|
||||
ukf_cholesky_solve(filter->Pyy, m, filter->sigma_y, filter->Pxy, l, filter->gain);
|
||||
|
||||
// restore PM matrix
|
||||
|
||||
for(i = 0 ; i < l * l ; i++)
|
||||
filter->P[i]= filter->PM_save[i];
|
||||
|
||||
// update state
|
||||
|
||||
for(j = 0 ; j < m ; j++)
|
||||
filter->dy[j] = y[j] - filter->ym[j];
|
||||
for(i = 0 ; i < l ; i++) {
|
||||
filter->x[i] = filter->xm[i];
|
||||
t = 0.0;
|
||||
for(j = 0 ; j < m ; j++)
|
||||
t += filter->gain[i * m + j] * filter->dy[j];
|
||||
filter->x[i] += t;
|
||||
}
|
||||
|
||||
for(i = 0 ; i < l ; i++)
|
||||
for(j = 0 ; j < m ; j++) {
|
||||
t = 0.0;
|
||||
for(k = j ; k < m ; k++)
|
||||
t += filter->gain[i * m + k] * filter->Pyy[k * m + j];
|
||||
filter->KL[i * m + j] = t;
|
||||
}
|
||||
for(i = 0 ; i < l ; i++)
|
||||
for(j = 0 ; j < l ; j++) {
|
||||
t = 0.0;
|
||||
for(k = 0 ; k < m ; k++)
|
||||
t += filter->KL[i * m + k] * filter->KL[j * m + k];
|
||||
filter->P[i * l + j ] -= t;
|
||||
}
|
||||
|
||||
// finished with kalman iteration !
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,76 @@
|
||||
#ifndef UKF_H_
|
||||
#define UKF_H_
|
||||
|
||||
/*
|
||||
* Opaque structure holding filter information
|
||||
*/
|
||||
typedef struct ukf_filter_t *ukf_filter;
|
||||
/*
|
||||
* Basic functions describing evolution and measure
|
||||
*/
|
||||
typedef void (*filter_function)(double*, double *,double *);
|
||||
typedef void (*measure_function)(double *, double *);
|
||||
|
||||
/*
|
||||
* creates a new unscented kalman filter structure
|
||||
* @param state_dim: size of state variable
|
||||
* @param mesaure_dim: size of measurement
|
||||
* @param Q: additive model noise covariance matrix
|
||||
* @param R: additive measurement noise covariance matrix
|
||||
* @param ffun: the funcion describing the filter evolution equation
|
||||
* @param mfun: the measurement function
|
||||
*/
|
||||
ukf_filter
|
||||
ukf_filter_new(unsigned state_dim,
|
||||
unsigned measure_dim,
|
||||
double *Q,
|
||||
double *R,
|
||||
filter_function ffun,
|
||||
measure_function mfun);
|
||||
|
||||
/*
|
||||
* free filter memory
|
||||
* @param filter: the filter to delete
|
||||
*/
|
||||
void
|
||||
ukf_filter_delete(ukf_filter filter);
|
||||
|
||||
/*
|
||||
* set filter weight using default procedure
|
||||
* @param alpha: spread parameter
|
||||
* @param k: scaling parameter
|
||||
* @param beta: distribution fitting parameter (2 for Gaussian)
|
||||
*/
|
||||
void
|
||||
ukf_filter_compute_weights(ukf_filter filter,
|
||||
double alpha,
|
||||
double k,
|
||||
double beta);
|
||||
|
||||
/*
|
||||
* Reset filter to new state
|
||||
* @param x0: initial state
|
||||
* @param P0: initial state covariance matrix
|
||||
*/
|
||||
void
|
||||
ukf_filter_reset(ukf_filter filter,
|
||||
double *x0,
|
||||
double *PO);
|
||||
|
||||
/*
|
||||
* Get filter state
|
||||
* @param x: A vector that will hold the state
|
||||
* @param P: A vector that will hold the state covariance
|
||||
*/
|
||||
void
|
||||
ukf_filter_get_state(ukf_filter filter, double *x, double *P);
|
||||
|
||||
/*
|
||||
* Update filter using a measure
|
||||
* @param y: The measure vector
|
||||
* @param u: the command
|
||||
*/
|
||||
void
|
||||
ukf_filter_update(ukf_filter filter, double *y, double *u);
|
||||
|
||||
#endif /*UKF_H_*/
|
||||
@@ -0,0 +1,11 @@
|
||||
This directory contains examples of kalman filters applied to a couple of attitude determinatation problems.
|
||||
|
||||
tilt is a simple one axis angle problem - It fuses the data of a gyroscope and a two axis accelerometer to estimate an angle and the bias of the gyro.
|
||||
|
||||
ahrs_euler estimates 3 eulers angles and 3 gyro biases - it exhibits the problems of singularities associated with euler angles, but the additive noise hypothesys of our kalman filter can be verified.
|
||||
|
||||
ahrs_quat estimates a quaternion and 3 gyro biases - It doesn't have the problem of singularity of euler angles, but the additiv noise hypothesis is violated as it would make the quaternion non unitary.
|
||||
|
||||
The _optim versions are "highly optimized" versions of the filters designed with a performance goal in mind when running on a microcontroller.
|
||||
|
||||
Big thanx to the gurus : Trammel Hudson, Aaron Khan, Stephane Puechmorel and
|
||||
@@ -0,0 +1,17 @@
|
||||
0/ tilt
|
||||
|
||||
|
||||
1/ ahrs_euler
|
||||
|
||||
ahrs_euler_ekf_compm:
|
||||
euler angles + bias estimation using ekf filter and complete measurements ( all 3 euler angles for every update)
|
||||
|
||||
ahrs_euler_ekf_seqm:
|
||||
euler angles + bias estimation using ekf filter and sequential measurements ( one of each euler angle every update)
|
||||
|
||||
|
||||
2/ ahrs_quat
|
||||
|
||||
ahrs_quat_ekf_compm:
|
||||
quaternion + bias estimation using ekf filter and complete measurements ( all 3 euler angles for every update)
|
||||
|
||||
@@ -0,0 +1,109 @@
|
||||
clear();
|
||||
getf('rotations.sci');
|
||||
getf('imu.sci');
|
||||
getf('ahrs_euler_utils.sci');
|
||||
getf('ekf.sci');
|
||||
|
||||
use_sim = 1;
|
||||
predict_only = 0;
|
||||
predict_discrete = 0;
|
||||
use_state_for_psi = 0;
|
||||
|
||||
if (use_sim),
|
||||
rand('seed', 0);
|
||||
getf('quadrotor.sci');
|
||||
true_euler0 = [ 0.01; 0.2; 0.4];
|
||||
dt = 0.015625;
|
||||
[time, true_rates, true_eulers] = quadrotor_gen_roll_step(true_euler0, dt);
|
||||
[accel, mag, gyro] = imu_sim(time, true_rates, true_eulers);
|
||||
else
|
||||
//filename = "../data/log_ahrs_test";
|
||||
//filename = "../data/log_ahrs_bug";
|
||||
//filename = "../data/log_ahrs_roll";
|
||||
filename = "../data/log_ahrs_yaw_pitched";
|
||||
[time, accel, mag, gyro] = imu_read_log(filename);
|
||||
end
|
||||
|
||||
dt = time(2) - time(1);
|
||||
[X0] = ahrs_euler_init(150, accel, mag);
|
||||
|
||||
// initial state covariance
|
||||
P0e = 1.;
|
||||
P0b = 1.;
|
||||
|
||||
P0 = [ P0e 0 0 0 0 0
|
||||
0 P0e 0 0 0 0
|
||||
0 0 P0e 0 0 0
|
||||
0 0 0 P0b 0 0
|
||||
0 0 0 0 P0b 0
|
||||
0 0 0 0 0 P0b ];
|
||||
|
||||
// process covariance noise
|
||||
Qe = 0.0;
|
||||
Qb = 0.008;
|
||||
|
||||
Q = [ Qe 0 0 0 0 0
|
||||
0 Qe 0 0 0 0
|
||||
0 0 Qe 0 0 0
|
||||
0 0 0 Qb 0 0
|
||||
0 0 0 0 Qb 0
|
||||
0 0 0 0 0 Qb ];
|
||||
|
||||
// measurement covariance noise
|
||||
R = [
|
||||
1.3^2 0. 0.
|
||||
0. 1.3^2 0.
|
||||
0. 0. 2.5^2
|
||||
];
|
||||
|
||||
X=[X0]; // state
|
||||
P=[P0]; // state covariance
|
||||
M=[X0(1:3)]; // measurements
|
||||
|
||||
|
||||
for i=2:length(time)
|
||||
// command
|
||||
rate_i_ = gyro(:,i-1) - X(4:6,i-1);
|
||||
// state
|
||||
Xi_ = X(:, i-1);
|
||||
// state time derivative
|
||||
Xdoti_ = ahrs_euler_get_Xdot(Xi_, rate_i_);
|
||||
// process covariance
|
||||
Pi_ = ahrs_euler_get_P(P, i-1);
|
||||
// Jacobian of state time derivative wrt state
|
||||
Fi_ = ahrs_euler_get_F(Xi_, rate_i_);
|
||||
|
||||
if predict_discrete
|
||||
[Xpred, Ppred] = ekf_predict_discrete(Xi_, Xdoti_, dt, Pi_, Fi_, Q);
|
||||
else
|
||||
[Xpred, Ppred] = ekf_predict_continuous(Xi_, Xdoti_, dt, Pi_, Fi_, Q);
|
||||
end
|
||||
|
||||
if use_state_for_psi
|
||||
m_phi = phi_of_accel(accel(:, i));
|
||||
m_theta = theta_of_accel(accel(:, i));
|
||||
m_psi = psi_of_mag(Xpred(1), Xpred(2), mag(:, i));
|
||||
measure_i1 = [ m_phi; m_theta; m_psi];
|
||||
else
|
||||
measure_i1 = euler_of_accel_mag(accel(:, i), mag(:, i));
|
||||
end
|
||||
|
||||
M = [M measure_i1];
|
||||
|
||||
if predict_only,
|
||||
X = [X Xpred];
|
||||
P = [P Ppred];
|
||||
else
|
||||
err = measure_i1 - Xpred(1:3);
|
||||
|
||||
H = ahrs_euler_compm_get_deuler_dX(Xpred);
|
||||
|
||||
[Xup, Pup] = ekf_update(Xpred, Ppred, H, R, err);
|
||||
|
||||
X = [X Xup];
|
||||
P = [P Pup];
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
ahrs_euler_display(time, X, P, M)
|
||||
@@ -0,0 +1,116 @@
|
||||
clear();
|
||||
getf('rotations.sci');
|
||||
getf('imu.sci');
|
||||
getf('ahrs_euler_utils.sci');
|
||||
getf('ekf.sci');
|
||||
|
||||
use_sim = 1;
|
||||
|
||||
if (use_sim),
|
||||
getf('quadrotor.sci');
|
||||
true_euler0 = [ 0.01; 0.2; 0.4];
|
||||
dt = 0.015625;
|
||||
[time, true_rates, true_eulers] = quadrotor_gen_roll_step(true_euler0, dt);
|
||||
[accel, mag, gyro] = imu_sim(time, true_rates, true_eulers);
|
||||
else
|
||||
//filename = "../data/log_ahrs_test";
|
||||
//filename = "../data/log_ahrs_bug";
|
||||
filename = "../data/log_ahrs_roll";
|
||||
//filename = "../data/log_ahrs_yaw_pitched";
|
||||
[time, accel, mag, gyro] = imu_read_log(filename);
|
||||
end
|
||||
|
||||
AHRS_STEP_PHI = 0;
|
||||
AHRS_STEP_THETA = 1;
|
||||
AHRS_STEP_PSI = 2;
|
||||
AHRS_STEP_NB = 3;
|
||||
|
||||
dt = time(2) - time(1);
|
||||
[X0] = ahrs_euler_init(150, accel, mag);
|
||||
|
||||
// initial state covariance
|
||||
P0e = 1.;
|
||||
P0b = 1.;
|
||||
|
||||
P0 = [ P0e 0 0 0 0 0
|
||||
0 P0e 0 0 0 0
|
||||
0 0 P0e 0 0 0
|
||||
0 0 0 P0b 0 0
|
||||
0 0 0 0 P0b 0
|
||||
0 0 0 0 0 P0b ];
|
||||
|
||||
// process covariance noise
|
||||
Qe = 0.000;
|
||||
Qb = 0.008;
|
||||
|
||||
Q = [ Qe 0 0 0 0 0
|
||||
0 Qe 0 0 0 0
|
||||
0 0 Qe 0 0 0
|
||||
0 0 0 Qb 0 0
|
||||
0 0 0 0 Qb 0
|
||||
0 0 0 0 0 Qb ];
|
||||
|
||||
// measure covariance noise
|
||||
R = [
|
||||
1.3^2 0. 0.
|
||||
0. 1.3^2 0.
|
||||
0. 0. 2.5^2
|
||||
];
|
||||
|
||||
X=[X0];
|
||||
P=[P0];
|
||||
M=[X0(1:3)];
|
||||
|
||||
for i=2:length(time)
|
||||
// command
|
||||
rate_i_ = gyro(:,i-1) - X(4:6,i-1);
|
||||
// state
|
||||
Xi_ = X(:, i-1);
|
||||
// state time derivative
|
||||
Xdoti_ = ahrs_euler_get_Xdot(Xi_, rate_i_);
|
||||
// process covariance
|
||||
Pi_ = ahrs_euler_get_P(P, i-1);
|
||||
// Jacobian of state time derivative wrt state
|
||||
Fi_ = ahrs_euler_get_F(Xi_, rate_i_);
|
||||
|
||||
[Xpred, Ppred] = ekf_predict_continuous(Xi_, Xdoti_, dt, Pi_, Fi_, Q);
|
||||
|
||||
// X = [X Xpred];
|
||||
// P = [P Ppred];
|
||||
|
||||
ahrs_state = modulo(i, AHRS_STEP_NB);
|
||||
measure_i1 = euler_of_accel_mag(accel(:, i), mag(:, i));
|
||||
select ahrs_state,
|
||||
case AHRS_STEP_PHI,
|
||||
measure = phi_of_accel(accel(:,i));
|
||||
estimate = Xpred(1);
|
||||
H = [1 0 0 0 0 0];
|
||||
R = [1.3^2];
|
||||
case AHRS_STEP_THETA,
|
||||
measure = theta_of_accel(accel(:,i));
|
||||
estimate = Xpred(2);
|
||||
H = [0 1 0 0 0 0];
|
||||
R = [1.3^2];
|
||||
case AHRS_STEP_PSI,
|
||||
phi = phi_of_accel(accel(:,i));
|
||||
theta = theta_of_accel(accel(:,i));
|
||||
measure = psi_of_mag(Xpred(1), Xpred(2), mag(:,i));
|
||||
// measure = psi_of_mag(phi, theta, mag(:,i));
|
||||
estimate = Xpred(3);
|
||||
H = [0 0 1 0 0 0];
|
||||
R = [2.5^2];
|
||||
end
|
||||
|
||||
M = [M measure_i1];
|
||||
|
||||
err = measure - estimate;
|
||||
|
||||
[Xup, Pup] = ekf_update(Xpred, Ppred, H, R, err);
|
||||
|
||||
X = [X Xup];
|
||||
P = [P Pup];
|
||||
|
||||
|
||||
end
|
||||
|
||||
ahrs_euler_display(time, X, P, M)
|
||||
@@ -0,0 +1,157 @@
|
||||
//
|
||||
//
|
||||
//
|
||||
// Initialisation
|
||||
//
|
||||
//
|
||||
//
|
||||
function [X0] = ahrs_euler_init(avg_len, accel, mag, gyro)
|
||||
|
||||
avg_accel = sum(accel(:,1:avg_len), 'c') / avg_len;
|
||||
avg_mag = sum(mag(:,1:avg_len), 'c') / avg_len;
|
||||
avg_gyro = sum(gyro(:,1:avg_len), 'c') / avg_len;
|
||||
|
||||
phi0 = phi_of_accel(avg_accel);
|
||||
theta0 = theta_of_accel(avg_accel);
|
||||
psi0 = psi_of_mag(phi0, theta0, avg_mag);
|
||||
|
||||
|
||||
X0 = [ phi0 theta0 psi0 avg_gyro(1) avg_gyro(2) avg_gyro(3) ]';
|
||||
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Filter
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
function [Pi] = ahrs_euler_get_P(P, i)
|
||||
Pi = P(:, 1+6*(i-1):6*i);
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Display
|
||||
//
|
||||
//
|
||||
//
|
||||
function [] = ahrs_euler_display(time, X, P, M)
|
||||
xbasc();
|
||||
subplot(3,1,1)
|
||||
xtitle('Angle');
|
||||
EstAngleDeg = X(1:3,:) * 180 / %pi;
|
||||
MeasAngleDeg = M * 180 / %pi;
|
||||
plot2d([time; time; time]', EstAngleDeg', style=[5, 3, 2], leg="phi@theta@psi");
|
||||
plot2d([time; time; time]', MeasAngleDeg', style=[0, 0, 0]);
|
||||
|
||||
subplot(3,1,2)
|
||||
xtitle('Bias');
|
||||
EstBiasDeg = X(4:6,:) * 180 / %pi;
|
||||
plot2d([time; time; time]', EstBiasDeg', style=[5, 3, 2], leg="phi@theta@psi");
|
||||
|
||||
subplot(3,1,3)
|
||||
xtitle('Covariance');
|
||||
|
||||
Pdiag = [];
|
||||
for i=1:length(time)
|
||||
Pi = ahrs_euler_get_P(P, i);
|
||||
Pdiag = [Pdiag [Pi(1,1) Pi(2,2) Pi(3,3) Pi(4,4) Pi(5,5) Pi(6,6)]'];
|
||||
end
|
||||
plot2d([time; time; time; time; time; time]', Pdiag', style=[5, 3, 2, 0, 0, 0],...
|
||||
leg="phi@theta@psi@bp@bq@br");
|
||||
|
||||
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Time derivative of state
|
||||
//
|
||||
//
|
||||
//
|
||||
function [Xdot] = ahrs_euler_get_Xdot(X, rate)
|
||||
|
||||
phi = X(1);
|
||||
theta = X(2);
|
||||
psi = X(3);
|
||||
|
||||
euler_dot = [ 1 sin(phi)*tan(theta) cos(phi)*tan(theta)
|
||||
0 cos(phi) -sin(phi)
|
||||
0 sin(phi)/cos(theta) cos(phi)/cos(theta)
|
||||
] * rate;
|
||||
Xdot = [ euler_dot; 0; 0; 0];
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Derivative of Xdot wrt X
|
||||
//
|
||||
//
|
||||
//
|
||||
function [F] = ahrs_euler_get_F(X, rate)
|
||||
phi = X(1);
|
||||
theta = X(2);
|
||||
psi = X(3);
|
||||
|
||||
p = rate(1);
|
||||
q = rate(2);
|
||||
r = rate(3);
|
||||
|
||||
d_phidot_dX = [ cos(phi)*tan(theta)*q - sin(phi)*tan(theta)*r
|
||||
1/(1+theta^2) * (sin(phi)*q+cos(phi)*r)
|
||||
0
|
||||
-1
|
||||
-sin(phi)*tan(theta)
|
||||
-cos(phi)*tan(theta) ]';
|
||||
|
||||
d_thetadot_dX = [ -sin(phi)*q - cos(phi)*r
|
||||
0
|
||||
0
|
||||
0
|
||||
-cos(phi)
|
||||
sin(phi) ]';
|
||||
|
||||
d_psidot_dX = [ cos(phi)/cos(theta)*q - sin(phi)/cos(theta)*r
|
||||
sin(theta)/cos(theta)^2*(sin(phi)*q+cos(phi)*r)
|
||||
0
|
||||
0
|
||||
-sin(phi)/cos(theta)
|
||||
-cos(phi)/cos(theta)
|
||||
]';
|
||||
|
||||
F = [ d_phidot_dX
|
||||
d_thetadot_dX
|
||||
d_psidot_dX
|
||||
0 0 0 0 0 0
|
||||
0 0 0 0 0 0
|
||||
0 0 0 0 0 0
|
||||
];
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Derivative of measure wrt state
|
||||
//
|
||||
//
|
||||
//
|
||||
function [H] = ahrs_euler_compm_get_deuler_dX(X)
|
||||
|
||||
H = [
|
||||
1 0 0 0 0 0
|
||||
0 1 0 0 0 0
|
||||
0 0 1 0 0 0
|
||||
];
|
||||
|
||||
endfunction
|
||||
@@ -0,0 +1,146 @@
|
||||
clear();
|
||||
getf('rotations.sci');
|
||||
getf('imu.sci');
|
||||
getf('ahrs_euler_utils.sci');
|
||||
getf('ahrs_quat_utils.sci');
|
||||
getf('ekf.sci');
|
||||
|
||||
use_sim = 1;
|
||||
|
||||
if (use_sim),
|
||||
getf('quadrotor.sci');
|
||||
true_euler0 = [ 0.01; 0.25; 0.5];
|
||||
dt = 0.015625;
|
||||
[time, true_rates, true_eulers] = quadrotor_gen_roll_step(true_euler0, dt);
|
||||
[accel, mag, gyro] = imu_sim(time, true_rates, true_eulers);
|
||||
else
|
||||
//filename = "../data/log_ahrs_test";
|
||||
//filename = "../data/log_ahrs_bug";
|
||||
filename = "../data/log_ahrs_roll";
|
||||
//filename = "../data/log_ahrs_yaw_pitched";
|
||||
[time, accel, mag, gyro] = imu_read_log(filename);
|
||||
end
|
||||
|
||||
|
||||
AHRS_STEP_PHI = 0;
|
||||
AHRS_STEP_THETA = 1;
|
||||
AHRS_STEP_PSI = 2;
|
||||
AHRS_STEP_NB = 3;
|
||||
|
||||
[m_eulers] = ahrs_compute_euler_measurements(accel, mag);
|
||||
|
||||
dt = time(2) - time(1);
|
||||
[X0] = ahrs_quat_init(150, accel, mag);
|
||||
|
||||
// initial state covariance matrix
|
||||
P0q = 1.;
|
||||
P0b = .1;
|
||||
|
||||
P0 = [ P0q 0 0 0 0 0 0
|
||||
0 P0q 0 0 0 0 0
|
||||
0 0 P0q 0 0 0 0
|
||||
0 0 0 P0q 0 0 0
|
||||
0 0 0 0 P0b 0 0
|
||||
0 0 0 0 0 P0b 0
|
||||
0 0 0 0 0 0 P0b ];
|
||||
|
||||
|
||||
Qq = 0.;
|
||||
Qb = 0.008;
|
||||
|
||||
Q = [ Qq 0 0 0 0 0 0
|
||||
0 Qq 0 0 0 0 0
|
||||
0 0 Qq 0 0 0 0
|
||||
0 0 0 Qq 0 0 0
|
||||
0 0 0 0 Qb 0 0
|
||||
0 0 0 0 0 Qb 0
|
||||
0 0 0 0 0 0 Qb ];
|
||||
|
||||
X=[X0];
|
||||
est_eulers = [euler_of_quat(X0(1:4))'];
|
||||
//P=[P0];
|
||||
P = P0;
|
||||
|
||||
for i=1:length(time)-1
|
||||
|
||||
q0 = X(1, i);
|
||||
q1 = X(2, i);
|
||||
q2 = X(3, i);
|
||||
q3 = X(4, i);
|
||||
|
||||
p = gyro(1,i) - X(5, i);
|
||||
q = gyro(2,i) - X(6, i);
|
||||
r = gyro(3,i) - X(7, i);
|
||||
|
||||
OMEGA = 1/2 * [ 0 -p -q -r 0 0 0
|
||||
p 0 r -q 0 0 0
|
||||
q -r 0 p 0 0 0
|
||||
r q -p 0 0 0 0
|
||||
0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 ];
|
||||
Xdot = OMEGA * X(:,i);
|
||||
|
||||
F = 1/2 * [ 0 -p -q -r q1 q2 q3;
|
||||
p 0 r -q -q0 q3 -q2;
|
||||
q -r 0 p -q3 q0 q1;
|
||||
r q -p 0 q2 -q1 -q0;
|
||||
0 0 0 0 0 0 0;
|
||||
0 0 0 0 0 0 0;
|
||||
0 0 0 0 0 0 0 ];
|
||||
|
||||
[X1, P] = ekf_predict_continuous(X(:, i), Xdot, dt, P, F, Q);
|
||||
X1 = normalise_quat(X1);
|
||||
// X = [X X1];
|
||||
// P = [P P1];
|
||||
est_euler = euler_of_quat(X1(1:4))';
|
||||
est_eulers = [est_eulers est_euler];
|
||||
|
||||
ahrs_state = modulo(i, AHRS_STEP_NB);
|
||||
est_euler = euler_of_quat(X1);
|
||||
H = [0;0;0;0;0;0;0]';
|
||||
measure = 0;
|
||||
estimate = 0;
|
||||
|
||||
select ahrs_state,
|
||||
case AHRS_STEP_PHI,
|
||||
measure = phi_of_accel(accel(:,i));
|
||||
estimate = est_euler(1);
|
||||
H = ahrs_quat_get_dphi_dX(X1);
|
||||
R = [1.3^2];
|
||||
case AHRS_STEP_THETA,
|
||||
measure = theta_of_accel(accel(:,i));
|
||||
estimate = est_euler(2);
|
||||
H = ahrs_quat_get_dtheta_dX(X1);
|
||||
R = [1.3^2];
|
||||
case AHRS_STEP_PSI,
|
||||
measure = psi_of_mag(m_eulers(1,i), m_eulers(2,i), mag(:,i));
|
||||
estimate = est_euler(3);
|
||||
H = ahrs_quat_get_dpsi_dX(X1);
|
||||
R = [2.5^2];
|
||||
end
|
||||
|
||||
err = measure - estimate;
|
||||
[X2 P] = ekf_update(X1, P, H, R, err);
|
||||
|
||||
X = [X X2];
|
||||
|
||||
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
||||
xbasc();
|
||||
subplot(3,1,1)
|
||||
xtitle('Rates');
|
||||
plot2d([time; time; time]', gyro', style=[5 3 2], leg="p@q@r");
|
||||
|
||||
subplot(3,1,2)
|
||||
plot2d([time;time;time]', m_eulers', style=[5 3 2], leg="phi@theta@psi");
|
||||
xtitle('Angles');
|
||||
plot2d([time;time;time]', est_eulers', style=[0 0 0]);
|
||||
|
||||
subplot(3,1,3)
|
||||
xtitle('Biases');
|
||||
plot2d([time; time; time]', X(5:7, :)', style=[5 3 2], leg="p@q@r");
|
||||
@@ -0,0 +1,195 @@
|
||||
//
|
||||
//
|
||||
//
|
||||
// Initialisation
|
||||
//
|
||||
//
|
||||
//
|
||||
function [X0] = ahrs_quat_init(avg_len, accel, mag, gyro)
|
||||
|
||||
[AEX0] = ahrs_euler_init(avg_len, accel, mag, gyro);
|
||||
|
||||
[quat] = quat_of_euler(AEX0(1:3));
|
||||
|
||||
X0 = [ quat; AEX0(4:6) ];
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Filter
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
function [Pi] = ahrs_quat_get_P(P, i)
|
||||
Pi = P(:, 1+7*(i-1):7*i);
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Time derivative of state
|
||||
//
|
||||
//
|
||||
//
|
||||
function [Xdot] = ahrs_quat_get_Xdot(X, rate)
|
||||
p = rate(1);
|
||||
q = rate(2);
|
||||
r = rate(3);
|
||||
OMEGA = 1/2 * [ 0 -p -q -r 0 0 0
|
||||
p 0 r -q 0 0 0
|
||||
q -r 0 p 0 0 0
|
||||
r q -p 0 0 0 0
|
||||
0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 0 ];
|
||||
Xdot = OMEGA * X;
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Derivative of Xdot wrt X
|
||||
//
|
||||
//
|
||||
//
|
||||
function [F] = ahrs_quat_get_F(X, rate)
|
||||
q0 = X(1);
|
||||
q1 = X(2);
|
||||
q2 = X(3);
|
||||
q3 = X(4);
|
||||
|
||||
p = rate(1);
|
||||
q = rate(2);
|
||||
r = rate(3);
|
||||
|
||||
F = 1/2 * [ 0 -p -q -r q1 q2 q3;
|
||||
p 0 r -q -q0 q3 -q2;
|
||||
q -r 0 p -q3 q0 q1;
|
||||
r q -p 0 q2 -q1 -q0;
|
||||
0 0 0 0 0 0 0;
|
||||
0 0 0 0 0 0 0;
|
||||
0 0 0 0 0 0 0 ];
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Derivative of measure wrt state
|
||||
//
|
||||
//
|
||||
//
|
||||
function [H] = ahrs_quat_get_dphi_dX(X)
|
||||
|
||||
DCM = dcm_of_quat(X(1:4));
|
||||
phi_err = 2 / (DCM(3,3)^2 + DCM(2,3)^2);
|
||||
q0 = X(1);
|
||||
q1 = X(2);
|
||||
q2 = X(3);
|
||||
q3 = X(4);
|
||||
H = [
|
||||
(q1 * DCM(3,3)) * phi_err
|
||||
(q0 * DCM(3,3) + 2 * q1 * DCM(2,3)) * phi_err
|
||||
(q3 * DCM(3,3) + 2 * q2 * DCM(2,3)) * phi_err
|
||||
(q2 * DCM(3,3)) * phi_err
|
||||
0
|
||||
0
|
||||
0
|
||||
]';
|
||||
|
||||
endfunction
|
||||
|
||||
function [H] = ahrs_quat_get_dtheta_dX(X)
|
||||
|
||||
DCM = dcm_of_quat(X(1:4));
|
||||
theta_err = 2 / sqrt(1 - DCM(1,3)^2);
|
||||
q0 = X(1);
|
||||
q1 = X(2);
|
||||
q2 = X(3);
|
||||
q3 = X(4);
|
||||
H = [
|
||||
q2 * theta_err
|
||||
-q3 * theta_err
|
||||
q0 * theta_err
|
||||
-q1 * theta_err
|
||||
0
|
||||
0
|
||||
0
|
||||
]';
|
||||
|
||||
endfunction
|
||||
|
||||
function [H] = ahrs_quat_get_dpsi_dX(X)
|
||||
|
||||
DCM = dcm_of_quat(X(1:4));
|
||||
psi_err = 2 / (DCM(1,1)^2 + DCM(1,2)^2);
|
||||
q0 = X(1);
|
||||
q1 = X(2);
|
||||
q2 = X(3);
|
||||
q3 = X(4);
|
||||
H = [
|
||||
(q3 * DCM(1,1)) * psi_err
|
||||
(q2 * DCM(1,1)) * psi_err
|
||||
(q1 * DCM(1,1) + 2 * q2 * DCM(1,2)) * psi_err
|
||||
(q0 * DCM(1,1) + 2 * q3 * DCM(1,2)) * psi_err
|
||||
0
|
||||
0
|
||||
0
|
||||
]';
|
||||
|
||||
endfunction
|
||||
|
||||
function [H] = ahrs_quat_compm_get_deuler_dX(X)
|
||||
|
||||
Hphi = ahrs_quat_get_dphi_dX(X);
|
||||
Htheta = ahrs_quat_get_dtheta_dX(X);
|
||||
Hpsi = ahrs_quat_get_dpsi_dX(X);
|
||||
H = [ Hphi; Htheta; Hpsi];
|
||||
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
// Display
|
||||
//
|
||||
//
|
||||
//
|
||||
function [] = ahrs_quat_display(time, X, P, M)
|
||||
xbasc();
|
||||
subplot(3,1,1)
|
||||
xtitle('Angle');
|
||||
EstEulerDeg = [];
|
||||
for i=1:length(time)
|
||||
ead = euler_of_quat(X(1:4, i)) * 180 / %pi;
|
||||
EstEulerDeg = [EstEulerDeg ead];
|
||||
end
|
||||
MeasAngleDeg = M * 180 / %pi;
|
||||
plot2d([time; time; time]', EstEulerDeg', style=[5, 3, 2], leg="phi@theta@psi");
|
||||
plot2d([time; time; time]', MeasAngleDeg', style=[0, 0, 0]);
|
||||
|
||||
subplot(3,1,2)
|
||||
xtitle('Bias');
|
||||
EstBiasDeg = X(5:7,:) * 180 / %pi;
|
||||
plot2d([time; time; time]', EstBiasDeg', style=[5, 3, 2], leg="phi@theta@psi");
|
||||
|
||||
subplot(3,1,3)
|
||||
xtitle('Covariance');
|
||||
|
||||
Pdiag = [];
|
||||
for i=1:length(time)
|
||||
Pi = ahrs_quat_get_P(P, i);
|
||||
Pdiag = [Pdiag [Pi(1,1) Pi(2,2) Pi(3,3) Pi(4,4) Pi(5,5) Pi(6,6) Pi(7,7)]'];
|
||||
end
|
||||
plot2d([time; time; time; time; time; time; time]', Pdiag', style=[5, 4, 3, 2, 0, 0, 0],...
|
||||
leg="q0@q1@q2@q3@bp@bq@br");
|
||||
|
||||
endfunction
|
||||
|
||||
@@ -0,0 +1,169 @@
|
||||
clear();
|
||||
clf();
|
||||
|
||||
//##########
|
||||
//##########
|
||||
function [rm_x, rm_y, rm_z, ra_x, ra_y, ra_z] = read_log(filename)
|
||||
|
||||
rm_x=[];
|
||||
rm_y=[];
|
||||
rm_z=[];
|
||||
|
||||
ra_x=[];
|
||||
ra_y=[];
|
||||
ra_z=[];
|
||||
|
||||
u=mopen(filename,'r');
|
||||
|
||||
while meof(u) == 0,
|
||||
line = mgetl(u, 1);
|
||||
if strindex(line, '#') ~= 1 & length(line) ~= 0,
|
||||
[nb_scan, _mz, _my, _mx] = msscanf(1, line, '148 IMU_MAG_RAW %f %f %f');
|
||||
if nb_scan == 3,
|
||||
rm_x = [rm_x _mx];
|
||||
rm_y = [rm_y _my];
|
||||
rm_z = [rm_z _mz];
|
||||
else
|
||||
[nb_scan, _ax, _ay, _az] = msscanf(1, line, '148 IMU_ACCEL_RAW %f %f %f');
|
||||
if nb_scan == 3,
|
||||
ra_x = [ra_x _ax];
|
||||
ra_y = [ra_y _ay];
|
||||
ra_z = [ra_z _az];
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
mclose(u);
|
||||
|
||||
endfunction
|
||||
|
||||
//##########
|
||||
//##########
|
||||
function [nx, ny, nz, gx, gy, gz] = min_max_calib(dx, dy, dz)
|
||||
min_x = min(dx);
|
||||
max_x = max(dx);
|
||||
nx = (max_x + min_x) / 2.;
|
||||
gx = (max_x - min_x) / 2.;
|
||||
|
||||
min_y = min(dy);
|
||||
max_y = max(dy);
|
||||
ny = (max_y + min_y) / 2.;
|
||||
gy = (max_y - min_y) / 2.;
|
||||
|
||||
min_z = min(dz);
|
||||
max_z = max(dz);
|
||||
nz = (max_z + min_z) / 2.;
|
||||
gz = (max_z - min_z) / 2.;
|
||||
endfunction
|
||||
|
||||
[rm_x, rm_y, rm_z, ra_x, ra_y, ra_z] = read_log('log_magnetometer');
|
||||
|
||||
[m_mm_nx, m_mm_ny, m_mm_nz, m_mm_gx, m_mm_gy, m_mm_gz] = ...
|
||||
min_max_calib(rm_x, rm_y, rm_z);
|
||||
|
||||
[a_mm_nx, a_mm_ny, a_mm_nz, a_mm_gx, a_mm_gy, a_mm_gz] = ...
|
||||
min_max_calib(ra_x, ra_y, ra_z);
|
||||
|
||||
m_mm_x = (rm_x - m_mm_nx) / m_mm_gx;
|
||||
m_mm_y = (rm_y - m_mm_ny) / m_mm_gy;
|
||||
m_mm_z = (rm_z - m_mm_nz) / m_mm_gz;
|
||||
|
||||
printf('mx : n -> %f g -> %f\n', m_mm_nx, m_mm_gx);
|
||||
printf('my : n -> %f g -> %f\n', m_mm_ny, m_mm_gy);
|
||||
printf('mz : n -> %f g -> %f\n', m_mm_nz, m_mm_gz);
|
||||
|
||||
m_mm_norm = sqrt(m_mm_x^2 + m_mm_y^2 + m_mm_z^2);
|
||||
idx_m = 1:length(rm_z);
|
||||
|
||||
|
||||
//a_mm_x = (ra_x - a_mm_nx) / a_mm_gx;
|
||||
//a_mm_y = (ra_y - a_mm_ny) / a_mm_gy;
|
||||
//a_mm_z = (ra_z - a_mm_nz) / a_mm_gz;
|
||||
|
||||
//a_mm_norm = sqrt(a_mm_x^2 + a_mm_y^2 + a_mm_z^2);
|
||||
//idx_a = 1:length(ra_z);
|
||||
|
||||
subplot(3,2,1);
|
||||
param3d(rm_x, rm_y, rm_z);
|
||||
subplot(3,2,2);
|
||||
plot(idx_m, rm_x);
|
||||
|
||||
//subplot(3,2,2);
|
||||
//param3d(ra_x, ra_y, ra_z);
|
||||
subplot(3,2,3);
|
||||
plot(idx_m, m_mm_norm, idx_m, ones(1, length(rm_z)));
|
||||
//subplot(3,2,4);
|
||||
//plot(idx_a, a_mm_norm, idx_a, ones(1, length(ra_z)));
|
||||
|
||||
subplot(3,2,5);
|
||||
alpha = 0:0.1:2*%pi;
|
||||
c_x = m_mm_nx+ m_mm_gx * cos(alpha);
|
||||
c_y = m_mm_ny+ m_mm_gy * sin(alpha);
|
||||
plot(rm_x, rm_y, c_x, c_y)
|
||||
|
||||
//subplot(3,2,6);
|
||||
//c_x = a_mm_nx+ a_mm_gx * cos(alpha);
|
||||
//c_y = a_mm_ny+ a_mm_gy * sin(alpha);
|
||||
//plot(ra_x, ra_y, c_x, c_y)
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
function err = cost_fun(p, z)
|
||||
err = (z(1) - p(1))^2 / p(4)^2 + ...
|
||||
(z(2) - p(2))^2 / p(5)^2 + ...
|
||||
(z(3) - p(3))^2 / p(6)^2 - 1;
|
||||
endfunction
|
||||
|
||||
if 0
|
||||
p0 = [ mx_neutral; my_neutral; mz_neutral; mx_gain; my_gain; mz_gain];
|
||||
Z = [rm_x; rm_y; rm_z];
|
||||
[p, err] = datafit(cost_fun, Z, p0)
|
||||
else
|
||||
// expe 0
|
||||
p = [ 1925.0482
|
||||
2124.7774
|
||||
1975.155
|
||||
446.40699
|
||||
426.51219
|
||||
412.88952 ];
|
||||
// expe 1 cut
|
||||
p = [ 1933.2636
|
||||
2136.651
|
||||
1977.4088
|
||||
464.52769
|
||||
452.71114
|
||||
433.81507 ];
|
||||
// expe 2 uncut
|
||||
p = [ 1931.7855
|
||||
2114.7077
|
||||
1976.6819
|
||||
444.15179
|
||||
472.04251
|
||||
479.76116 ];
|
||||
// expe 2 cut
|
||||
p = [ 1931.1511
|
||||
2145.2051
|
||||
1977.5055
|
||||
467.83817
|
||||
450.52012
|
||||
447.26642 ];
|
||||
|
||||
end
|
||||
|
||||
opt_nx = p(1);
|
||||
opt_ny = p(2);
|
||||
opt_nz = p(3);
|
||||
opt_gx = p(4);
|
||||
opt_gy = p(5);
|
||||
opt_gz = p(6);
|
||||
opt_mx = (rm_x - opt_nx) / opt_gx;
|
||||
opt_my = (rm_y - opt_ny) / opt_gy;
|
||||
opt_mz = (rm_z - opt_nz) / opt_gz;
|
||||
|
||||
//opt_norm = sqrt(opt_mx^2 + opt_my^2 + opt_mz^2);
|
||||
//subplot(2,2,3);
|
||||
//plot(idx, opt_norm, 'cya+');
|
||||
@@ -0,0 +1,34 @@
|
||||
clear();
|
||||
getf('rotations.sci');
|
||||
getf('imu.sci');
|
||||
|
||||
|
||||
rand('seed', 0);
|
||||
getf('quadrotor.sci');
|
||||
true_euler0 = [ 0.0; 0.0; 0.0];
|
||||
dt = 0.015625;
|
||||
|
||||
[time, true_rates, true_eulers] = quadrotor_gen_roll_step(true_euler0, dt);
|
||||
[accel, mag, gyro] = imu_sim_misaligned(time, true_rates, true_eulers);
|
||||
|
||||
|
||||
|
||||
|
||||
xbasc();
|
||||
subplot(3,1,1)
|
||||
plot2d([time]', accel', style=[5, 3, 2], leg="a_x@a_y@a_z");
|
||||
xtitle( 'Accel', 's', 'volts') ;
|
||||
|
||||
|
||||
true_rates_deg = deg_of_rad(true_rates);
|
||||
subplot(3,1,2)
|
||||
plot2d([time]', true_rates_deg', style=[5, 3, 2], leg="rate_p@rate_q@rate_r");
|
||||
xtitle( 'True rates', 's', 'deg/s') ;
|
||||
|
||||
subplot(3,1,3)
|
||||
plot2d([time]', gyro', style=[5, 3, 2], leg="g_x@g_y@g_z");
|
||||
xtitle( 'Raw gyros', 's', 'adc') ;
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,60 @@
|
||||
clear();
|
||||
exec("calibration_utils.sci");
|
||||
|
||||
//[raw_mag, raw_accel] = read_log("log_calib_mag_4.dat");
|
||||
[raw_mag, raw_accel] = read_log("log_calib_mag_b2a2");
|
||||
|
||||
//[fraw_mag] = filter_noise(raw_mag,15,300);
|
||||
fraw_mag = raw_mag;
|
||||
|
||||
|
||||
n0 = [ 2000; 2000; 2000];
|
||||
g0 = [ 4; 4; 4];
|
||||
|
||||
[scaled_mag] = scale_sensor(fraw_mag, g0, n0);
|
||||
[scaled_module] = compute_mod(scaled_mag);
|
||||
|
||||
|
||||
function err = cost_fun(p, z)
|
||||
err = (z(1) - p(1))^2 * p(4)^2 + ...
|
||||
(z(2) - p(2))^2 * p(5)^2 + ...
|
||||
(z(3) - p(3))^2 * p(6)^2 - (2^11)^2;
|
||||
endfunction
|
||||
|
||||
[p, err] = datafit(cost_fun, fraw_mag, [n0; g0]);
|
||||
|
||||
[scaled_mag2] = scale_sensor(fraw_mag, p(4:6), p(1:3));
|
||||
[scaled_module2] = compute_mod(scaled_mag2);
|
||||
|
||||
|
||||
gain_foo = [ 2^11/p(4)
|
||||
2^11/p(5)
|
||||
2^11/p(6) ]
|
||||
|
||||
clf();
|
||||
|
||||
subplot(4,1,1);
|
||||
[nl, nc] = size(raw_mag);
|
||||
plot2d(1:nc, raw_mag(1,:), 1);
|
||||
plot2d(1:nc, raw_mag(2,:), 2);
|
||||
plot2d(1:nc, raw_mag(3,:), 3);
|
||||
xtitle('raw sensors');
|
||||
|
||||
subplot(4,1,2);
|
||||
[nl, nc] = size(fraw_mag);
|
||||
plot2d(1:nc, scaled_mag(1,:), 1);
|
||||
plot2d(1:nc, scaled_mag(2,:), 2);
|
||||
plot2d(1:nc, scaled_mag(3,:), 3);
|
||||
xtitle('scaled sensors initial guess');
|
||||
|
||||
subplot(4,1,3);
|
||||
plot2d(1:nc, scaled_mag2(1,:), 1);
|
||||
plot2d(1:nc, scaled_mag2(2,:), 2);
|
||||
plot2d(1:nc, scaled_mag2(3,:), 3);
|
||||
xtitle('scaled sensors optimised');
|
||||
|
||||
subplot(4,1,4);
|
||||
plot2d(1:nc, scaled_module, 1);
|
||||
subplot(4,1,4);
|
||||
plot2d(1:nc, scaled_module2, 2);
|
||||
xtitle('norm');
|
||||
@@ -0,0 +1,65 @@
|
||||
function [raw_mag, raw_accel] = read_log(filename)
|
||||
|
||||
raw_mag = [];
|
||||
raw_accel = [];
|
||||
|
||||
u=mopen(filename,'r');
|
||||
|
||||
while meof(u) == 0,
|
||||
line = mgetl(u, 1);
|
||||
if strindex(line, '#') ~= 1 & length(line) ~= 0,
|
||||
[nb_scan, _time, _mx, _my, _mz] = msscanf(1, line, '%f 151 IMU_MAG_RAW %f %f %f');
|
||||
if nb_scan == 4,
|
||||
raw_mag = [raw_mag [_mx _my _mz]'];
|
||||
else
|
||||
[nb_scan, _time, _ax, _ay, _az] = msscanf(1, line, '%f 149 IMU_ACCEL_RAW %f %f %f');
|
||||
if nb_scan == 4,
|
||||
raw_accel = [raw_accel [_ax _ay _az]'];
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
mclose(u);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
function [fraw_sensor] = filter_noise(raw_sensor, window_size, max_var)
|
||||
|
||||
[nl, nc] = size(raw_sensor);
|
||||
fraw_sensor = [];
|
||||
|
||||
for i=window_size+1:nc-window_size-1
|
||||
|
||||
v = variance(raw_sensor(:,i-window_size:i+window_size),'c');
|
||||
|
||||
if norm(v) < max_var
|
||||
fraw_sensor = [fraw_sensor raw_sensor(:,i)];
|
||||
end
|
||||
end
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function [scaled_sensor] = scale_sensor(raw_sensor, g, n)
|
||||
|
||||
[nl, nc] = size(raw_sensor);
|
||||
scaled_sensor = zeros(nl, nc);
|
||||
|
||||
for i=1:nc
|
||||
scaled_sensor(:,i) = g .* (raw_sensor(:,i) - n);
|
||||
end
|
||||
|
||||
endfunction
|
||||
|
||||
function [_mod] = compute_mod(sensor)
|
||||
|
||||
[nl, nc] = size(sensor);
|
||||
_mod = zeros(1,nc);
|
||||
for i=1:nc
|
||||
_mod(i) = norm(sensor(:,i));
|
||||
end
|
||||
|
||||
endfunction
|
||||
@@ -0,0 +1,70 @@
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
function [X1, P1] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q)
|
||||
|
||||
X1 = X0 + X0dot * dt;
|
||||
P0dot = F*P0 + P0*F' + Q;
|
||||
P1 = P0 + P0dot * dt;
|
||||
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
function [X1, P1] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q)
|
||||
|
||||
X1 = X0 + X0dot * dt;
|
||||
expF = expm(dt * F);
|
||||
P0dot = expF*P0*expF' + Q;
|
||||
P1 = P0 + P0dot * dt;
|
||||
|
||||
endfunction
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
function [X1, P1] = ekf_update(X0, P0, H, R, err)
|
||||
|
||||
E = H * P0 * H' + R;
|
||||
K = P0 * H' * inv(E);
|
||||
P1 = P0 - K * H * P0;
|
||||
X1 = X0 + K * err;
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
//
|
||||
// Pade approximation of matrix exponential
|
||||
//
|
||||
function [expA] = mat_exp(A, epsilon)
|
||||
|
||||
normA = norm(A, 'inf');
|
||||
//Ns = max(0, int(normA)) ???
|
||||
Ns = normA;
|
||||
ki = 2^(-Ns) * A;
|
||||
|
||||
i=1;
|
||||
// foo = 2^(3-2*i)*
|
||||
|
||||
|
||||
expA = A;
|
||||
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
x0 = %pi/180.;
|
||||
|
||||
x = x0;
|
||||
n = 20;
|
||||
m = 31;
|
||||
|
||||
a(1) = floor(x);
|
||||
for i = 2:n,
|
||||
x = 1 / (x - a(i-1));
|
||||
a(i) = floor(x);
|
||||
end
|
||||
|
||||
N = [1 a(1)];
|
||||
D = [0 1];
|
||||
|
||||
for i = 2:n,
|
||||
N(i+1) = a(i)*N(i) + N(i-1);
|
||||
D(i+1) = a(i)*D(i) + D(i-1);
|
||||
if N(i+1)/D(i+1) == x0,
|
||||
break
|
||||
end
|
||||
if N(i+1) > 2^m | D(i+1) > 2^m,
|
||||
N(i+1) = N(i);
|
||||
D(i+1) = D(i);
|
||||
break
|
||||
end
|
||||
end
|
||||
|
||||
y = N(i+1)/D(i+1);
|
||||
|
||||
//N
|
||||
//D
|
||||
printf('%0.20f, delta=%e, %d',y,y-x0,i)
|
||||
printf('%20f',N(i+1))
|
||||
printf('%20f',D(i+1))
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user