mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-04 21:23:57 +08:00
Added kalman_demo. This is an attitude/position EKF example.
This commit is contained in:
jgoppert
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,121 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file KalmanNav.hpp
|
||||
*
|
||||
* kalman filter navigation code
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
//#define MATRIX_ASSERT
|
||||
//#define VECTOR_ASSERT
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <systemlib/math/Vector.hpp>
|
||||
#include <systemlib/math/Vector3.hpp>
|
||||
#include <systemlib/math/Matrix.hpp>
|
||||
#include <systemlib/math/Quaternion.hpp>
|
||||
#include <systemlib/math/Dcm.hpp>
|
||||
#include <systemlib/math/EulerAngles.hpp>
|
||||
#include <systemlib/control/blocks.hpp>
|
||||
#include <systemlib/control/block/BlockParam.hpp>
|
||||
#include <systemlib/control/block/UOrbSubscription.hpp>
|
||||
#include <systemlib/control/block/UOrbPublication.hpp>
|
||||
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <poll.h>
|
||||
#include <unistd.h>
|
||||
|
||||
class KalmanNav : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
KalmanNav(SuperBlock *parent, const char *name);
|
||||
virtual ~KalmanNav() {};
|
||||
void update();
|
||||
virtual void updatePublications();
|
||||
void predictFast(float dt);
|
||||
void predictSlow(float dt);
|
||||
void correctAtt();
|
||||
void correctGps();
|
||||
virtual void updateParams();
|
||||
protected:
|
||||
math::Matrix F;
|
||||
math::Matrix G;
|
||||
math::Matrix P;
|
||||
math::Matrix V;
|
||||
math::Matrix HAtt;
|
||||
math::Matrix RAtt;
|
||||
math::Matrix HGps;
|
||||
math::Matrix RGps;
|
||||
math::Dcm C_nb;
|
||||
math::Quaternion q;
|
||||
control::UOrbSubscription<sensor_combined_s> _sensors;
|
||||
control::UOrbSubscription<vehicle_gps_position_s> _gps;
|
||||
control::UOrbSubscription<parameter_update_s> _param_update;
|
||||
control::UOrbPublication<vehicle_global_position_s> _pos;
|
||||
control::UOrbPublication<vehicle_attitude_s> _att;
|
||||
uint64_t _pubTimeStamp;
|
||||
uint64_t _fastTimeStamp;
|
||||
uint64_t _slowTimeStamp;
|
||||
uint64_t _attTimeStamp;
|
||||
uint64_t _outTimeStamp;
|
||||
uint16_t _navFrames;
|
||||
float fN, fE, fD;
|
||||
// states
|
||||
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
|
||||
float phi, theta, psi;
|
||||
float vN, vE, vD;
|
||||
double lat, lon, alt;
|
||||
control::BlockParam<float> _vGyro;
|
||||
control::BlockParam<float> _vAccel;
|
||||
control::BlockParam<float> _rMag;
|
||||
control::BlockParam<float> _rGpsV;
|
||||
control::BlockParam<float> _rGpsGeo;
|
||||
control::BlockParam<float> _rGpsAlt;
|
||||
control::BlockParam<float> _rAccel;
|
||||
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
|
||||
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
|
||||
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
|
||||
void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
|
||||
int32_t getAltE3() { return int32_t(alt * 1.0e3); }
|
||||
void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
|
||||
};
|
||||
@@ -0,0 +1,42 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Basic example application
|
||||
#
|
||||
|
||||
APPNAME = kalman_demo
|
||||
PRIORITY = SCHED_PRIORITY_MAX - 30
|
||||
STACKSIZE = 2048
|
||||
|
||||
include $(APPDIR)/mk/app.mk
|
||||
@@ -0,0 +1,152 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Example User <mail@example.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file kalman_demo.cpp
|
||||
* Demonstration of control library
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
#include "KalmanNav.hpp"
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
extern "C" __EXPORT int kalman_demo_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int kalman_demo_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p <additional params>]\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int kalman_demo_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("kalman_demo already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn("kalman_demo",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
4096,
|
||||
kalman_demo_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tkalman_demo app is running\n");
|
||||
|
||||
} else {
|
||||
printf("\tkalman_demo app not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int kalman_demo_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
printf("[kalman_demo] starting\n");
|
||||
|
||||
using namespace math;
|
||||
|
||||
thread_running = true;
|
||||
|
||||
KalmanNav nav(NULL, "KF");
|
||||
|
||||
while (!thread_should_exit) {
|
||||
nav.update();
|
||||
}
|
||||
|
||||
printf("[kalman_demo] exiting.\n");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
|
||||
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_MAG, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_V, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_GEO, 1.0e-7f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.01f);
|
||||
@@ -65,11 +65,10 @@ CONFIGURED_APPS += systemcmds/delay_test
|
||||
# https://pixhawk.ethz.ch/px4/dev/deamon
|
||||
# CONFIGURED_APPS += examples/px4_deamon_app
|
||||
|
||||
# Math library
|
||||
# Library examples
|
||||
CONFIGURED_APPS += examples/math_demo
|
||||
|
||||
# Control library
|
||||
CONFIGURED_APPS += examples/control_demo
|
||||
CONFIGURED_APPS += examples/kalman_demo
|
||||
|
||||
# Shared object broker; required by many parts of the system.
|
||||
CONFIGURED_APPS += uORB
|
||||
|
||||
Reference in New Issue
Block a user