mirror of
https://github.com/0xFEEDC0DE64/arduino-esp32.git
synced 2025-06-30 12:30:59 +02:00
IDF release/v4.4 6a7d83af19
* Update toolchain to 2021r2 (#5828) * Update C3 libs to support only ECO3 and newer
This commit is contained in:
Me No Dev
GitHub
208
tools/sdk/esp32/include/esp-dsp/modules/kalman/ekf/include/ekf.h
Normal file
208
tools/sdk/esp32/include/esp-dsp/modules/kalman/ekf/include/ekf.h
Normal file
@ -0,0 +1,208 @@
|
||||
// Copyright 2020-2021 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
|
||||
#ifndef _ekf_h_
|
||||
#define _ekf_h_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <mat.h>
|
||||
|
||||
class ekf {
|
||||
public:
|
||||
// x - amount of states in EKF. x[n] = F*x[n-1] + G*u + W. Size of matrix F
|
||||
// w - amount of control measurements and noise inputs. Size of matrix G
|
||||
|
||||
ekf(int x, int w);
|
||||
|
||||
virtual ~ekf();
|
||||
virtual void Process(float *u, float dt);
|
||||
|
||||
virtual void Init() = 0;
|
||||
// x[n] = F*x[n-1] + G*u + W
|
||||
int NUMX; // number of states, X is the state vector (size of F matrix)
|
||||
int NUMW; // size of G matrix
|
||||
|
||||
// System state vector
|
||||
dspm::Mat &X;
|
||||
|
||||
// linearized system matrices
|
||||
dspm::Mat &F;
|
||||
dspm::Mat &G;
|
||||
|
||||
// covariance matrix and state vector
|
||||
dspm::Mat &P;
|
||||
|
||||
// input noise and measurement noise variances
|
||||
dspm::Mat &Q;
|
||||
|
||||
/**
|
||||
* Runge-Kutta state update method.
|
||||
* The method calculates derivatives of input vector x and control measurements u
|
||||
* Re
|
||||
* @param[in] x: state vector
|
||||
* @param[in] u: control measurement
|
||||
* @param[in] dt: time interval from last update in seconds
|
||||
*/
|
||||
void RungeKutta(dspm::Mat &x, float *u, float dt);
|
||||
|
||||
// System Dependent methods:
|
||||
|
||||
/**
|
||||
* Derivative of state vector X
|
||||
* Re
|
||||
* @param[in] x: state vector
|
||||
* @param[in] u: control measurement
|
||||
* @return
|
||||
* - derivative of input vector x and u
|
||||
*/
|
||||
virtual dspm::Mat StateXdot(dspm::Mat &x, float *u) = 0;
|
||||
/**
|
||||
* Calculation of system state matrices F and G
|
||||
* @param[in] x: state vector
|
||||
* @param[in] u: control measurement
|
||||
*/
|
||||
virtual void LinearizeFG(dspm::Mat &x, float *u) = 0;
|
||||
//
|
||||
|
||||
// System independent methods
|
||||
|
||||
/**
|
||||
* Calculates covariance prediction matrux P.
|
||||
* Update matrix P
|
||||
* @param[in] dt: time interval from last update
|
||||
*/
|
||||
virtual void CovariancePrediction(float dt);
|
||||
|
||||
/**
|
||||
* Update of current state by measured values.
|
||||
* Optimized method for non correlated values
|
||||
* Calculate Kalman gain and update matrix P and vector X.
|
||||
* @param[in] H: derivative matrix
|
||||
* @param[in] measured: array of measured values
|
||||
* @param[in] expected: array of expected values
|
||||
* @param[in] R: measurement noise covariance values
|
||||
*/
|
||||
virtual void Update(dspm::Mat &H, float *measured, float *expected, float *R);
|
||||
/**
|
||||
* Update of current state by measured values.
|
||||
* This method just as a reference for research purpose.
|
||||
* Not used in real calculations.
|
||||
* @param[in] H: derivative matrix
|
||||
* @param[in] measured: array of measured values
|
||||
* @param[in] expected: array of expected values
|
||||
* @param[in] R: measurement noise covariance values
|
||||
*/
|
||||
virtual void UpdateRef(dspm::Mat &H, float *measured, float *expected, float *R);
|
||||
|
||||
|
||||
float *HP;
|
||||
float *Km;
|
||||
|
||||
public:
|
||||
// Additional universal helper methods
|
||||
/**
|
||||
* Convert quaternion to rotation matrix.
|
||||
* @param[in] q: quaternion
|
||||
*
|
||||
* @return
|
||||
* - rotation matrix 3x3
|
||||
*/
|
||||
static dspm::Mat quat2rotm(float q[4]);
|
||||
|
||||
/**
|
||||
* Convert rotation matrix to quaternion.
|
||||
* @param[in] R: rotation matrix
|
||||
*
|
||||
* @return
|
||||
* - quaternion 4x1
|
||||
*/
|
||||
static dspm::Mat rotm2quat(dspm::Mat &R);
|
||||
|
||||
/**
|
||||
* Convert quaternion to Euler angels.
|
||||
* @param[in] R: quaternion
|
||||
*
|
||||
* @return
|
||||
* - Euler angels 3x1
|
||||
*/
|
||||
static dspm::Mat quat2eul(const float q[4]);
|
||||
/**
|
||||
* Convert Euler angels to rotation matrix.
|
||||
* @param[in] xyz: Euler angels
|
||||
*
|
||||
* @return
|
||||
* - rotation matrix 3x3
|
||||
*/
|
||||
static dspm::Mat eul2rotm(float xyz[3]);
|
||||
|
||||
/**
|
||||
* Convert rotation matrix to Euler angels.
|
||||
* @param[in] rotm: rotation matrix
|
||||
*
|
||||
* @return
|
||||
* - Euler angels 3x1
|
||||
*/
|
||||
static dspm::Mat rotm2eul(dspm::Mat &rotm);
|
||||
|
||||
/**
|
||||
* Df/dq: Derivative of vector by quaternion.
|
||||
* @param[in] vector: input vector
|
||||
* @param[in] quat: quaternion
|
||||
*
|
||||
* @return
|
||||
* - Derivative matrix 3x4
|
||||
*/
|
||||
static dspm::Mat dFdq(dspm::Mat &vector, dspm::Mat &quat);
|
||||
|
||||
/**
|
||||
* Df/dq: Derivative of vector by inverted quaternion.
|
||||
* @param[in] vector: input vector
|
||||
* @param[in] quat: quaternion
|
||||
*
|
||||
* @return
|
||||
* - Derivative matrix 3x4
|
||||
*/
|
||||
static dspm::Mat dFdq_inv(dspm::Mat &vector, dspm::Mat &quat);
|
||||
|
||||
/**
|
||||
* Make skew-symmetric matrix of vector.
|
||||
* @param[in] w: source vector
|
||||
*
|
||||
* @return
|
||||
* - skew-symmetric matrix 4x4
|
||||
*/
|
||||
static dspm::Mat SkewSym4x4(float *w);
|
||||
|
||||
// q product
|
||||
// Rl = [q(1) - q(2) - q(3) - q(4); ...
|
||||
// q(2) q(1) - q(4) q(3); ...
|
||||
// q(3) q(4) q(1) - q(2); ...
|
||||
// q(4) - q(3) q(2) q(1); ...
|
||||
|
||||
/**
|
||||
* Make right quaternion-product matrices.
|
||||
* @param[in] q: source quaternion
|
||||
*
|
||||
* @return
|
||||
* - right quaternion-product matrix 4x4
|
||||
*/
|
||||
static dspm::Mat qProduct(float *q);
|
||||
|
||||
};
|
||||
|
||||
#endif // _ekf_h_
|
||||
@ -0,0 +1,83 @@
|
||||
// Copyright 2020-2021 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef _ekf_imu13states_H_
|
||||
#define _ekf_imu13states_H_
|
||||
|
||||
#include "ekf.h"
|
||||
|
||||
/**
|
||||
* @brief This class is used to process and calculate attitude from imu sensors.
|
||||
*
|
||||
* The class use state vector with 13 follows values
|
||||
* X[0..3] - attitude quaternion
|
||||
* X[4..6] - gyroscope bias error, rad/sec
|
||||
* X[7..9] - magnetometer vector value - magn_ampl
|
||||
* X[10..12] - magnetometer offset value - magn_offset
|
||||
*
|
||||
* where, reference magnetometer value = magn_ampl*rotation_matrix' + magn_offset
|
||||
*/
|
||||
class ekf_imu13states: public ekf {
|
||||
public:
|
||||
ekf_imu13states();
|
||||
virtual ~ekf_imu13states();
|
||||
virtual void Init();
|
||||
|
||||
// Method calculates Xdot values depends on U
|
||||
// U - gyroscope values in radian per seconds (rad/sec)
|
||||
virtual dspm::Mat StateXdot(dspm::Mat &x, float *u);
|
||||
virtual void LinearizeFG(dspm::Mat &x, float *u);
|
||||
|
||||
// Methods for tests only.
|
||||
void Test();
|
||||
void TestFull(bool enable_att);
|
||||
|
||||
// Initial reference valies magnetometer and accelerometer
|
||||
dspm::Mat mag0;
|
||||
dspm::Mat accel0;
|
||||
|
||||
int NUMU; // number of control measurements
|
||||
|
||||
/**
|
||||
* Update part of system state by reference measurements accelerometer and magnetometer.
|
||||
* Only attitude and gyro bias will be updated.
|
||||
* This method should be used as main method after calibration.
|
||||
*
|
||||
* @param[in] accel_data: accelerometer measurement vector XYZ in g, where 1 g ~ 9.81 m/s^2
|
||||
* @param[in] magn_data: magnetometer measurement vector XYZ
|
||||
* @param[in] R: measurement noise covariance values for diagonal covariance matrix. Then smaller value, then more you trust them.
|
||||
*/
|
||||
void UpdateRefMeasurement(float *accel_data, float *magn_data, float R[6]);
|
||||
/**
|
||||
* Update full system state by reference measurements accelerometer and magnetometer.
|
||||
* This method should be used at calibration phase.
|
||||
*
|
||||
* @param[in] accel_data: accelerometer measurement vector XYZ in g, where 1 g ~ 9.81 m/s^2
|
||||
* @param[in] magn_data: magnetometer measurement vector XYZ
|
||||
* @param[in] R: measurement noise covariance values for diagonal covariance matrix. Then smaller value, then more you trust them.
|
||||
*/
|
||||
void UpdateRefMeasurementMagn(float *accel_data, float *magn_data, float R[6]);
|
||||
/**
|
||||
* Update system state by reference measurements accelerometer, magnetometer and attitude quaternion.
|
||||
* This method could be used when system on constant state or in initialization phase.
|
||||
* @param[in] accel_data: accelerometer measurement vector XYZ in g, where 1 g ~ 9.81 m/s^2
|
||||
* @param[in] magn_data: magnetometer measurement vector XYZ
|
||||
* @param[in] attitude: attitude quaternion
|
||||
* @param[in] R: measurement noise covariance values for diagonal covariance matrix. Then smaller value, then more you trust them.
|
||||
*/
|
||||
void UpdateRefMeasurement(float *accel_data, float *magn_data, float *attitude, float R[10]);
|
||||
|
||||
};
|
||||
|
||||
#endif // _ekf_imu13states_H_
|
||||
@ -52,6 +52,27 @@ public:
|
||||
* @param[in] src: source matrix
|
||||
*/
|
||||
Mat(const Mat &src);
|
||||
|
||||
/**
|
||||
* Make copy of matrix.
|
||||
* @param[in] src: source matrix
|
||||
* @param[in] row_pos: start row position of destination matrix
|
||||
* @param[in] col_pos: start col position of destination matrix
|
||||
*/
|
||||
void Copy(const Mat &src, int row_pos, int col_pos);
|
||||
|
||||
/**
|
||||
* Make copy of matrix.
|
||||
* @param[in] row_start: start row position of source matrix to copy
|
||||
* @param[in] row_size: size of wor elements of source matrix to copy
|
||||
* @param[in] col_start: start col position of source matrix to copy
|
||||
* @param[in] col_size: size of wor elements of source matrix to copy
|
||||
*
|
||||
* @return
|
||||
* - result matrix size row_size x col_size
|
||||
*/
|
||||
Mat Get(int row_start, int row_size, int col_start, int col_size);
|
||||
|
||||
/**
|
||||
* Copy operator
|
||||
*
|
||||
@ -356,10 +377,18 @@ public:
|
||||
int length; /*!< Total amount of data in data array*/
|
||||
|
||||
static float abs_tol; /*!< Max acceptable absolute tolerance*/
|
||||
|
||||
/**
|
||||
* Find determinant
|
||||
* @param[in] n: element number in first row
|
||||
*
|
||||
* @return
|
||||
* - determinant value
|
||||
*/
|
||||
float det(int n);
|
||||
private:
|
||||
Mat cofactor(int row, int col, int n);
|
||||
float det(int n);
|
||||
Mat adjoint();
|
||||
Mat cofactor(int row, int col, int n);
|
||||
Mat adjoint();
|
||||
|
||||
void allocate(); // Allocate buffer
|
||||
Mat expHelper(const Mat &m, int num);
|
||||
|
||||
Reference in New Issue
Block a user