mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 00:31:36 +08:00
delete exampales attitude_estimator_ekf
This commit is contained in:
Daniel Agar
@@ -1,298 +0,0 @@
|
||||
function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
|
||||
= AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
|
||||
|
||||
|
||||
%LQG Position Estimator and Controller
|
||||
% Observer:
|
||||
% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])
|
||||
% x[n+1|n] = Ax[n|n] + Bu[n]
|
||||
%
|
||||
% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $
|
||||
%
|
||||
%
|
||||
% Arguments:
|
||||
% approx_prediction: if 1 then the exponential map is approximated with a
|
||||
% first order taylor approximation. has at the moment not a big influence
|
||||
% (just 1st or 2nd order approximation) we should change it to rodriquez
|
||||
% approximation.
|
||||
% use_inertia_matrix: set to true if you have the inertia matrix J for your
|
||||
% quadrotor
|
||||
% xa_apo_k: old state vectotr
|
||||
% zFlag: if sensor measurement is available [gyro, acc, mag]
|
||||
% dt: dt in s
|
||||
% z: measurements [gyro, acc, mag]
|
||||
% q_rotSpeed: process noise gyro
|
||||
% q_rotAcc: process noise gyro acceleration
|
||||
% q_acc: process noise acceleration
|
||||
% q_mag: process noise magnetometer
|
||||
% r_gyro: measurement noise gyro
|
||||
% r_accel: measurement noise accel
|
||||
% r_mag: measurement noise mag
|
||||
% J: moment of inertia matrix
|
||||
|
||||
|
||||
% Output:
|
||||
% xa_apo: updated state vectotr
|
||||
% Pa_apo: updated state covariance matrix
|
||||
% Rot_matrix: rotation matrix
|
||||
% eulerAngles: euler angles
|
||||
% debugOutput: not used
|
||||
|
||||
|
||||
%% model specific parameters
|
||||
|
||||
|
||||
|
||||
% compute once the inverse of the Inertia
|
||||
persistent Ji;
|
||||
if isempty(Ji)
|
||||
Ji=single(inv(J));
|
||||
end
|
||||
|
||||
%% init
|
||||
persistent x_apo
|
||||
if(isempty(x_apo))
|
||||
gyro_init=single([0;0;0]);
|
||||
gyro_acc_init=single([0;0;0]);
|
||||
acc_init=single([0;0;-9.81]);
|
||||
mag_init=single([1;0;0]);
|
||||
x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]);
|
||||
|
||||
end
|
||||
|
||||
persistent P_apo
|
||||
if(isempty(P_apo))
|
||||
% P_apo = single(eye(NSTATES) * 1000);
|
||||
P_apo = single(200*ones(12));
|
||||
end
|
||||
|
||||
debugOutput = single(zeros(4,1));
|
||||
|
||||
%% copy the states
|
||||
wx= x_apo(1); % x body angular rate
|
||||
wy= x_apo(2); % y body angular rate
|
||||
wz= x_apo(3); % z body angular rate
|
||||
|
||||
wax= x_apo(4); % x body angular acceleration
|
||||
way= x_apo(5); % y body angular acceleration
|
||||
waz= x_apo(6); % z body angular acceleration
|
||||
|
||||
zex= x_apo(7); % x component gravity vector
|
||||
zey= x_apo(8); % y component gravity vector
|
||||
zez= x_apo(9); % z component gravity vector
|
||||
|
||||
mux= x_apo(10); % x component magnetic field vector
|
||||
muy= x_apo(11); % y component magnetic field vector
|
||||
muz= x_apo(12); % z component magnetic field vector
|
||||
|
||||
|
||||
|
||||
|
||||
%% prediction section
|
||||
% compute the apriori state estimate from the previous aposteriori estimate
|
||||
%body angular accelerations
|
||||
if (use_inertia_matrix==1)
|
||||
wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt;
|
||||
else
|
||||
wak =[wax;way;waz];
|
||||
end
|
||||
|
||||
%body angular rates
|
||||
wk =[wx; wy; wz] + dt*wak;
|
||||
|
||||
%derivative of the prediction rotation matrix
|
||||
O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]';
|
||||
|
||||
%prediction of the earth z vector
|
||||
if (approx_prediction==1)
|
||||
%e^(Odt)=I+dt*O+dt^2/2!O^2
|
||||
% so we do a first order approximation of the exponential map
|
||||
zek =(O*dt+single(eye(3)))*[zex;zey;zez];
|
||||
|
||||
else
|
||||
zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez];
|
||||
%zek =expm2(O*dt)*[zex;zey;zez]; not working because use double
|
||||
%precision
|
||||
end
|
||||
|
||||
|
||||
|
||||
%prediction of the magnetic vector
|
||||
if (approx_prediction==1)
|
||||
%e^(Odt)=I+dt*O+dt^2/2!O^2
|
||||
% so we do a first order approximation of the exponential map
|
||||
muk =(O*dt+single(eye(3)))*[mux;muy;muz];
|
||||
else
|
||||
muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz];
|
||||
%muk =expm2(O*dt)*[mux;muy;muz]; not working because use double
|
||||
%precision
|
||||
end
|
||||
|
||||
x_apr=[wk;wak;zek;muk];
|
||||
|
||||
% compute the apriori error covariance estimate from the previous
|
||||
%aposteriori estimate
|
||||
|
||||
EZ=[0,zez,-zey;
|
||||
-zez,0,zex;
|
||||
zey,-zex,0]';
|
||||
MA=[0,muz,-muy;
|
||||
-muz,0,mux;
|
||||
muy,-mux,0]';
|
||||
|
||||
E=single(eye(3));
|
||||
Z=single(zeros(3));
|
||||
|
||||
A_lin=[ Z, E, Z, Z
|
||||
Z, Z, Z, Z
|
||||
EZ, Z, O, Z
|
||||
MA, Z, Z, O];
|
||||
|
||||
A_lin=eye(12)+A_lin*dt;
|
||||
|
||||
%process covariance matrix
|
||||
|
||||
persistent Q
|
||||
if (isempty(Q))
|
||||
Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,...
|
||||
q_rotAcc,q_rotAcc,q_rotAcc,...
|
||||
q_acc,q_acc,q_acc,...
|
||||
q_mag,q_mag,q_mag]);
|
||||
end
|
||||
|
||||
P_apr=A_lin*P_apo*A_lin'+Q;
|
||||
|
||||
|
||||
%% update
|
||||
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
|
||||
|
||||
% R=[r_gyro,0,0,0,0,0,0,0,0;
|
||||
% 0,r_gyro,0,0,0,0,0,0,0;
|
||||
% 0,0,r_gyro,0,0,0,0,0,0;
|
||||
% 0,0,0,r_accel,0,0,0,0,0;
|
||||
% 0,0,0,0,r_accel,0,0,0,0;
|
||||
% 0,0,0,0,0,r_accel,0,0,0;
|
||||
% 0,0,0,0,0,0,r_mag,0,0;
|
||||
% 0,0,0,0,0,0,0,r_mag,0;
|
||||
% 0,0,0,0,0,0,0,0,r_mag];
|
||||
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
|
||||
%observation matrix
|
||||
%[zw;ze;zmk];
|
||||
H_k=[ E, Z, Z, Z;
|
||||
Z, Z, E, Z;
|
||||
Z, Z, Z, E];
|
||||
|
||||
y_k=z(1:9)-H_k*x_apr;
|
||||
|
||||
|
||||
%S_k=H_k*P_apr*H_k'+R;
|
||||
S_k=H_k*P_apr*H_k';
|
||||
S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
|
||||
K_k=(P_apr*H_k'/(S_k));
|
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k;
|
||||
P_apo=(eye(12)-K_k*H_k)*P_apr;
|
||||
else
|
||||
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0
|
||||
|
||||
R=[r_gyro,0,0;
|
||||
0,r_gyro,0;
|
||||
0,0,r_gyro];
|
||||
R_v=[r_gyro,r_gyro,r_gyro];
|
||||
%observation matrix
|
||||
|
||||
H_k=[ E, Z, Z, Z];
|
||||
|
||||
y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
|
||||
|
||||
% S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
|
||||
S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
|
||||
S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
|
||||
K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
|
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k;
|
||||
P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr;
|
||||
else
|
||||
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
|
||||
|
||||
% R=[r_gyro,0,0,0,0,0;
|
||||
% 0,r_gyro,0,0,0,0;
|
||||
% 0,0,r_gyro,0,0,0;
|
||||
% 0,0,0,r_accel,0,0;
|
||||
% 0,0,0,0,r_accel,0;
|
||||
% 0,0,0,0,0,r_accel];
|
||||
|
||||
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
|
||||
%observation matrix
|
||||
|
||||
H_k=[ E, Z, Z, Z;
|
||||
Z, Z, E, Z];
|
||||
|
||||
y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
|
||||
|
||||
% S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
|
||||
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
|
||||
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
|
||||
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
|
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k;
|
||||
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
|
||||
else
|
||||
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
|
||||
% R=[r_gyro,0,0,0,0,0;
|
||||
% 0,r_gyro,0,0,0,0;
|
||||
% 0,0,r_gyro,0,0,0;
|
||||
% 0,0,0,r_mag,0,0;
|
||||
% 0,0,0,0,r_mag,0;
|
||||
% 0,0,0,0,0,r_mag];
|
||||
R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
|
||||
%observation matrix
|
||||
|
||||
H_k=[ E, Z, Z, Z;
|
||||
Z, Z, Z, E];
|
||||
|
||||
y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
|
||||
|
||||
%S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
|
||||
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
|
||||
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
|
||||
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
|
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k;
|
||||
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
|
||||
else
|
||||
x_apo=x_apr;
|
||||
P_apo=P_apr;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
|
||||
%% euler anglels extraction
|
||||
z_n_b = -x_apo(7:9)./norm(x_apo(7:9));
|
||||
m_n_b = x_apo(10:12)./norm(x_apo(10:12));
|
||||
|
||||
y_n_b=cross(z_n_b,m_n_b);
|
||||
y_n_b=y_n_b./norm(y_n_b);
|
||||
|
||||
x_n_b=(cross(y_n_b,z_n_b));
|
||||
x_n_b=x_n_b./norm(x_n_b);
|
||||
|
||||
|
||||
xa_apo=x_apo;
|
||||
Pa_apo=P_apo;
|
||||
% rotation matrix from earth to body system
|
||||
Rot_matrix=[x_n_b,y_n_b,z_n_b];
|
||||
|
||||
|
||||
phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3));
|
||||
theta=-asin(Rot_matrix(1,3));
|
||||
psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1));
|
||||
eulerAngles=[phi;theta;psi];
|
||||
|
||||
@@ -1,45 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 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.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE examples__attitude_estimator_ekf
|
||||
MAIN attitude_estimator_ekf
|
||||
STACK_MAIN 1200
|
||||
COMPILE_FLAGS
|
||||
-Wno-float-equal
|
||||
SRCS
|
||||
attitude_estimator_ekf_main.cpp
|
||||
codegen/AttitudeEKF.c
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,130 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* 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 attitude_estimator_ekf_params.c
|
||||
*
|
||||
* Parameters for EKF filter
|
||||
*/
|
||||
|
||||
#include "attitude_estimator_ekf_params.h"
|
||||
#include <math.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
/* Extended Kalman Filter covariances */
|
||||
|
||||
|
||||
/**
|
||||
* Body angular rate process noise
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
|
||||
|
||||
/**
|
||||
* Body angular acceleration process noise
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
|
||||
|
||||
/**
|
||||
* Acceleration process noise
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
|
||||
|
||||
/**
|
||||
* Magnet field vector process noise
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
|
||||
|
||||
/**
|
||||
* Gyro measurement noise
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
|
||||
|
||||
/**
|
||||
* Accel measurement noise
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
|
||||
|
||||
/**
|
||||
* Mag measurement noise
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
|
||||
|
||||
/**
|
||||
* Moment of inertia matrix diagonal entry (1, 1)
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
* @unit kg*m^2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
|
||||
|
||||
/**
|
||||
* Moment of inertia matrix diagonal entry (2, 2)
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
* @unit kg*m^2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
|
||||
|
||||
/**
|
||||
* Moment of inertia matrix diagonal entry (3, 3)
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
* @unit kg*m^2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
|
||||
|
||||
/**
|
||||
* Moment of inertia enabled in estimator
|
||||
*
|
||||
* If set to != 0 the moment of inertia will be used in the estimator
|
||||
*
|
||||
* @group Attitude EKF estimator
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ATT_J_EN, 0);
|
||||
@@ -1,75 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* 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 attitude_estimator_ekf_params.h
|
||||
*
|
||||
* Parameters for EKF filter
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
struct attitude_estimator_ekf_params {
|
||||
float r[3];
|
||||
float q[4];
|
||||
float moment_inertia_J[9];
|
||||
int32_t use_moment_inertia;
|
||||
float roll_off;
|
||||
float pitch_off;
|
||||
float yaw_off;
|
||||
float mag_decl;
|
||||
int acc_comp;
|
||||
};
|
||||
|
||||
struct attitude_estimator_ekf_param_handles {
|
||||
param_t r0, r1, r2;
|
||||
param_t q0, q1, q2, q3;
|
||||
param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
|
||||
param_t use_moment_inertia;
|
||||
param_t mag_decl;
|
||||
param_t acc_comp;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
int parameters_init(struct attitude_estimator_ekf_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p);
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,26 +0,0 @@
|
||||
/*
|
||||
* AttitudeEKF.h
|
||||
*
|
||||
* Code generation for function 'AttitudeEKF'
|
||||
*
|
||||
* C source code generated on: Thu Aug 21 11:17:28 2014
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __ATTITUDEEKF_H__
|
||||
#define __ATTITUDEEKF_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "rtwtypes.h"
|
||||
#include "AttitudeEKF_types.h"
|
||||
|
||||
/* Function Declarations */
|
||||
extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]);
|
||||
extern void AttitudeEKF_initialize(void);
|
||||
extern void AttitudeEKF_terminate(void);
|
||||
#endif
|
||||
/* End of code generation (AttitudeEKF.h) */
|
||||
@@ -1,17 +0,0 @@
|
||||
/*
|
||||
* AttitudeEKF_types.h
|
||||
*
|
||||
* Code generation for function 'AttitudeEKF'
|
||||
*
|
||||
* C source code generated on: Thu Aug 21 11:17:28 2014
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __ATTITUDEEKF_TYPES_H__
|
||||
#define __ATTITUDEEKF_TYPES_H__
|
||||
|
||||
/* Include files */
|
||||
#include "rtwtypes.h"
|
||||
|
||||
#endif
|
||||
/* End of code generation (AttitudeEKF_types.h) */
|
||||
@@ -1,160 +0,0 @@
|
||||
/*
|
||||
* rtwtypes.h
|
||||
*
|
||||
* Code generation for function 'AttitudeEKF'
|
||||
*
|
||||
* C source code generated on: Thu Aug 21 11:17:28 2014
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RTWTYPES_H__
|
||||
#define __RTWTYPES_H__
|
||||
#ifndef TRUE
|
||||
# define TRUE (1U)
|
||||
#endif
|
||||
#ifndef FALSE
|
||||
# define FALSE (0U)
|
||||
#endif
|
||||
#ifndef __TMWTYPES__
|
||||
#define __TMWTYPES__
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
/*=======================================================================*
|
||||
* Target hardware information
|
||||
* Device type: ARM Compatible->ARM Cortex
|
||||
* Number of bits: char: 8 short: 16 int: 32
|
||||
* long: 32
|
||||
* native word size: 32
|
||||
* Byte ordering: LittleEndian
|
||||
* Signed integer division rounds to: Undefined
|
||||
* Shift right on a signed integer as arithmetic shift: on
|
||||
*=======================================================================*/
|
||||
|
||||
/*=======================================================================*
|
||||
* Fixed width word size data types: *
|
||||
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
|
||||
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
|
||||
* real32_T, real64_T - 32 and 64 bit floating point numbers *
|
||||
*=======================================================================*/
|
||||
|
||||
typedef signed char int8_T;
|
||||
typedef unsigned char uint8_T;
|
||||
typedef short int16_T;
|
||||
typedef unsigned short uint16_T;
|
||||
typedef int int32_T;
|
||||
typedef unsigned int uint32_T;
|
||||
typedef float real32_T;
|
||||
typedef double real64_T;
|
||||
|
||||
/*===========================================================================*
|
||||
* Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
|
||||
* ulong_T, char_T and byte_T. *
|
||||
*===========================================================================*/
|
||||
|
||||
typedef double real_T;
|
||||
typedef double time_T;
|
||||
typedef unsigned char boolean_T;
|
||||
typedef int int_T;
|
||||
typedef unsigned int uint_T;
|
||||
typedef unsigned long ulong_T;
|
||||
typedef char char_T;
|
||||
typedef char_T byte_T;
|
||||
|
||||
/*===========================================================================*
|
||||
* Complex number type definitions *
|
||||
*===========================================================================*/
|
||||
#define CREAL_T
|
||||
typedef struct {
|
||||
real32_T re;
|
||||
real32_T im;
|
||||
} creal32_T;
|
||||
|
||||
typedef struct {
|
||||
real64_T re;
|
||||
real64_T im;
|
||||
} creal64_T;
|
||||
|
||||
typedef struct {
|
||||
real_T re;
|
||||
real_T im;
|
||||
} creal_T;
|
||||
|
||||
typedef struct {
|
||||
int8_T re;
|
||||
int8_T im;
|
||||
} cint8_T;
|
||||
|
||||
typedef struct {
|
||||
uint8_T re;
|
||||
uint8_T im;
|
||||
} cuint8_T;
|
||||
|
||||
typedef struct {
|
||||
int16_T re;
|
||||
int16_T im;
|
||||
} cint16_T;
|
||||
|
||||
typedef struct {
|
||||
uint16_T re;
|
||||
uint16_T im;
|
||||
} cuint16_T;
|
||||
|
||||
typedef struct {
|
||||
int32_T re;
|
||||
int32_T im;
|
||||
} cint32_T;
|
||||
|
||||
typedef struct {
|
||||
uint32_T re;
|
||||
uint32_T im;
|
||||
} cuint32_T;
|
||||
|
||||
|
||||
/*=======================================================================*
|
||||
* Min and Max: *
|
||||
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
|
||||
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
|
||||
*=======================================================================*/
|
||||
|
||||
#define MAX_int8_T ((int8_T)(127))
|
||||
#define MIN_int8_T ((int8_T)(-128))
|
||||
#define MAX_uint8_T ((uint8_T)(255))
|
||||
#define MIN_uint8_T ((uint8_T)(0))
|
||||
#define MAX_int16_T ((int16_T)(32767))
|
||||
#define MIN_int16_T ((int16_T)(-32768))
|
||||
#define MAX_uint16_T ((uint16_T)(65535))
|
||||
#define MIN_uint16_T ((uint16_T)(0))
|
||||
#define MAX_int32_T ((int32_T)(2147483647))
|
||||
#define MIN_int32_T ((int32_T)(-2147483647-1))
|
||||
#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
|
||||
#define MIN_uint32_T ((uint32_T)(0))
|
||||
|
||||
/* Logical type definitions */
|
||||
#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
|
||||
# ifndef false
|
||||
# define false (0U)
|
||||
# endif
|
||||
# ifndef true
|
||||
# define true (1U)
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/*
|
||||
* MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
|
||||
* for signed integer values.
|
||||
*/
|
||||
#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
|
||||
#error "This code must be compiled using a 2's complement representation for signed integer values"
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Maximum length of a MATLAB identifier (function/variable)
|
||||
* including the null-termination character. Referenced by
|
||||
* rt_logging.c and rt_matrx.c.
|
||||
*/
|
||||
#define TMW_NAME_LENGTH_MAX 64
|
||||
|
||||
#endif
|
||||
#endif
|
||||
/* End of code generation (rtwtypes.h) */
|
||||
Reference in New Issue
Block a user