add some code

managed_components/espressif__esp-dsp/modules/kalman/ekf/CMakeLists.txt

@@ -0,0 +1,5 @@
+set(COMPONENT_SRCS  "common/ekf.cpp")
+
+set(COMPONENT_ADD_INCLUDEDIRS   "include")
+
+register_component()

managed_components/espressif__esp-dsp/modules/kalman/ekf/common/ekf.cpp

@@ -0,0 +1,415 @@
+// 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.
+
+#include "ekf.h"
+#include <float.h>
+
+ekf::ekf(int x, int w) : NUMX(x),
+    NUMW(w),
+    X(*new dspm::Mat(x, 1)),
+
+    F(*new dspm::Mat(x, x)),
+    G(*new dspm::Mat(x, w)),
+    P(*new dspm::Mat(x, x)),
+    Q(*new dspm::Mat(w, w))
+{
+
+    this->P *= 0;
+    this->Q *= 0;
+    this->X *= 0;
+    this->X.data[0] = 1; // direction to 0
+    this->HP = new float[this->NUMX];
+    this->Km = new float[this->NUMX];
+    for (size_t i = 0; i < this->NUMX; i++) {
+        this->HP[i] = 0;
+        this->Km[i] = 0;
+    }
+}
+
+ekf::~ekf()
+{
+    delete &X;
+    delete &F;
+    delete &G;
+    delete &P;
+    delete &Q;
+
+    delete this->HP;
+    delete this->Km;
+}
+
+void ekf::Process(float *u, float dt)
+{
+    this->LinearizeFG(this->X, (float *)u);
+    this->RungeKutta(this->X, u, dt);
+    this->CovariancePrediction(dt);
+}
+
+void ekf::RungeKutta(dspm::Mat &x, float *U, float dt)
+{
+
+    float dt2 = dt / 2.0f;
+
+    dspm::Mat Xlast = x;          // make a working copy
+    dspm::Mat K1 = StateXdot(x, U); // k1 = f(x, u)
+    x = Xlast + (K1 * dt2);
+
+    dspm::Mat K2 = StateXdot(x, U); // k2 = f(x + 0.5*dT*k1, u)
+    x = Xlast + K2 * dt2;
+
+    dspm::Mat K3 = StateXdot(x, U); // k3 = f(x + 0.5*dT*k2, u)
+    x = Xlast + K3 * dt;
+
+    dspm::Mat K4 = StateXdot(x, U); // k4 = f(x + dT * k3, u)
+
+    // Xnew = X + dT * (k1 + 2 * k2 + 2 * k3 + k4) / 6
+    x = Xlast + (K1 + 2.0f * K2 + 2.0f * K3 + K4) * (dt / 6.0f);
+}
+
+dspm::Mat ekf::SkewSym4x4(float w[3])
+{
+    //={    0,  -w[0],  -w[1],  -w[2],
+    //   w[0],      0,   w[2],  -w[1],
+    //   w[1],  -w[2],      0,   w[0],
+    //   w[2],   w[1],  -w[0],     0 };
+
+    dspm::Mat result(4, 4);
+    result.data[0] = 0;
+    result.data[1] = -w[0];
+    result.data[2] = -w[1];
+    result.data[3] = -w[2];
+
+    result.data[4] = w[0];
+    result.data[5] = 0;
+    result.data[6] = w[2];
+    result.data[7] = -w[1];
+
+    result.data[8] = w[1];
+    result.data[9] = -w[2];
+    result.data[10] = 0;
+    result.data[11] = w[0];
+
+    result.data[12] = w[2];
+    result.data[13] = w[1];
+    result.data[14] = -w[0];
+    result.data[15] = 0;
+
+    return result;
+}
+
+dspm::Mat ekf::qProduct(float *q)
+{
+    dspm::Mat result(4, 4);
+
+    result.data[0] = q[0];
+    result.data[1] = -q[1];
+    result.data[2] = -q[2];
+    result.data[3] = -q[3];
+
+    result.data[4] = q[1];
+    result.data[5] = q[0];
+    result.data[6] = -q[3];
+    result.data[7] = q[2];
+
+    result.data[8] = q[2];
+    result.data[9] = q[3];
+    result.data[10] = q[0];
+    result.data[11] = -q[1];
+
+    result.data[12] = q[3];
+    result.data[13] = -q[2];
+    result.data[14] = q[1];
+    result.data[15] = q[0];
+
+    return result;
+}
+
+void ekf::CovariancePrediction(float dt)
+{
+    dspm::Mat f = this->F * dt;
+
+    f = f + dspm::Mat::eye(this->NUMX);
+
+    dspm::Mat f_t = f.t();
+    this->P = ((f * this->P) * f_t) + (dt * dt) * ((G * Q) * G.t());
+}
+
+void ekf::Update(dspm::Mat &H, float *measured, float *expected, float *R)
+{
+    float HPHR, Error;
+    dspm::Mat Y(measured, H.rows, 1);
+    dspm::Mat Z(expected, H.rows, 1);
+
+    for (int m = 0; m < H.rows; m++) {
+        for (int j = 0; j < this->NUMX; j++) {
+            // Find Hp = H*P
+            HP[j] = 0;
+        }
+        for (int k = 0; k < this->NUMX; k++) {
+            for (int j = 0; j < this->NUMX; j++) {
+                // Find Hp = H*P
+                HP[j] += H(m, k) * P(k, j);
+            }
+        }
+        HPHR = R[m]; // Find  HPHR = H*P*H' + R
+        for (int k = 0; k < this->NUMX; k++) {
+            HPHR += HP[k] * H(m, k);
+        }
+        float invHPHR = 1.0f / HPHR;
+        for (int k = 0; k < this->NUMX; k++) {
+            Km[k] = HP[k] * invHPHR; // find K = HP/HPHR
+        }
+        for (int i = 0; i < this->NUMX; i++) {
+            // Find P(m)= P(m-1) + K*HP
+            for (int j = i; j < NUMX; j++) {
+                P(i, j) = P(j, i) = P(i, j) - Km[i] * HP[j];
+            }
+        }
+
+        Error = Y(m, 0) - Z(m, 0);
+        for (int i = 0; i < this->NUMX; i++) {
+            // Find X(m)= X(m-1) + K*Error
+            X(i, 0) = X(i, 0) + Km[i] * Error;
+        }
+    }
+}
+
+void ekf::UpdateRef(dspm::Mat &H, float *measured, float *expected, float *R)
+{
+    dspm::Mat h_t = H.t();
+    dspm::Mat S = H * P * h_t; // +diag(R);
+    for (size_t i = 0; i < H.rows; i++) {
+        S(i, i) += R[i];
+    }
+
+    dspm::Mat S_ = S.pinv(); // 1 / S
+
+    dspm::Mat K = (P * h_t) * S_;
+    this->P = (dspm::Mat::eye(this->NUMX) - K * H) * P;
+
+    dspm::Mat Y(measured, H.rows, 1);
+    dspm::Mat Z(expected, H.rows, 1);
+
+    dspm::Mat Err = Y - Z;
+    this->X += (K * Err);
+}
+
+dspm::Mat ekf::quat2rotm(float q[4])
+{
+    float q0 = q[0];
+    float q1 = q[1];
+    float q2 = q[2];
+    float q3 = q[3];
+    dspm::Mat Rm(3, 3);
+
+    Rm(0, 0) = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;
+    Rm(1, 0) = 2.0f * (q1 * q2 + q0 * q3);
+    Rm(2, 0) = 2.0f * (q1 * q3 - q0 * q2);
+    Rm(0, 1) = 2.0f * (q1 * q2 - q0 * q3);
+    Rm(1, 1) = (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3);
+    Rm(2, 1) = 2.0f * (q2 * q3 + q0 * q1);
+    Rm(0, 2) = 2.0f * (q1 * q3 + q0 * q2);
+    Rm(1, 2) = 2.0f * (q2 * q3 - q0 * q1);
+    Rm(2, 2) = (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
+
+    return Rm;
+}
+
+dspm::Mat ekf::quat2eul(const float q[4])
+{
+    dspm::Mat result(3, 1);
+    float R13, R11, R12, R23, R33;
+    float q0s = q[0] * q[0];
+    float q1s = q[1] * q[1];
+    float q2s = q[2] * q[2];
+    float q3s = q[3] * q[3];
+
+    R13 = 2.0f * (q[1] * q[3] + q[0] * q[2]);
+    R11 = q0s + q1s - q2s - q3s;
+    R12 = -2.0f * (q[1] * q[2] - q[0] * q[3]);
+    R23 = -2.0f * (q[2] * q[3] - q[0] * q[1]);
+    R33 = q0s - q1s - q2s + q3s;
+
+    result.data[1] = (asinf(R13));
+    result.data[2] = (atan2f(R12, R11));
+    result.data[0] = (atan2f(R23, R33));
+    return result;
+}
+
+dspm::Mat ekf::eul2rotm(float xyz[3])
+{
+    dspm::Mat result(3, 3);
+    float Cx = std::cos(xyz[0]);
+    float Sx = std::sin(xyz[0]);
+    float Cy = std::cos(xyz[1]);
+    float Sy = std::sin(xyz[1]);
+    float Cz = std::cos(xyz[2]);
+    float Sz = std::sin(xyz[2]);
+
+    result(0, 0) = Cy * Cz;
+    result(0, 1) = -Cy * Sz;
+    result(0, 2) = Sy;
+
+    result(1, 0) = Cz * Sx * Sy + Cx * Sz;
+    result(1, 1) = Cx * Cz - Sx * Sy * Sz;
+    result(1, 2) = -Cy * Sx;
+
+    result(2, 0) = -Cx * Cz * Sy + Sx * Sz;
+    result(2, 1) = Cz * Sx + Cx * Sy * Sz;
+    result(2, 2) = Cx * Cy;
+    return result;
+}
+
+#ifndef FLT_EPSILON
+#define FLT_EPSILON 1.192092896e-07F
+#endif // FLT_EPSILON
+
+dspm::Mat ekf::rotm2eul(dspm::Mat &rotm)
+{
+    dspm::Mat result(3, 1);
+    float x, y, z;
+//    float cy = sqrtf(rotm(2, 2) * rotm(2, 2) + rotm(2, 0) * rotm(2, 0));
+    float cy = sqrtf(rotm(2, 2) * rotm(2, 2) + rotm(1, 2) * rotm(1, 2));
+    if (cy > 16 * FLT_EPSILON) {
+        x = -atan2f(rotm(1, 2), rotm(2, 2));
+        y = -atan2f(-rotm(0, 2), (float)cy);
+        z = -atan2f(rotm(0, 1), rotm(0, 0));
+    } else {
+        z = -atan2f(-rotm(1, 0), rotm(1, 1));
+        y = -atan2f(-rotm(0, 2), (float)cy);
+        x = 0;
+    }
+    result(0, 0) = x;
+    result(1, 0) = y;
+    result(2, 0) = z;
+    return result;
+}
+
+static inline float SIGN(float x)
+{
+    return (x >= 0.0f) ? +1.0f : -1.0f;
+}
+
+dspm::Mat ekf::rotm2quat(dspm::Mat &m)
+{
+    float r11 = m(0, 0);
+    float r12 = m(0, 1);
+    float r13 = m(0, 2);
+    float r21 = m(1, 0);
+    float r22 = m(1, 1);
+    float r23 = m(1, 2);
+    float r31 = m(2, 0);
+    float r32 = m(2, 1);
+    float r33 = m(2, 2);
+    float q0 = (r11 + r22 + r33 + 1.0f) / 4.0f;
+    float q1 = (r11 - r22 - r33 + 1.0f) / 4.0f;
+    float q2 = (-r11 + r22 - r33 + 1.0f) / 4.0f;
+    float q3 = (-r11 - r22 + r33 + 1.0f) / 4.0f;
+    if (q0 < 0.0f) {
+        q0 = 0.0f;
+    }
+    if (q1 < 0.0f) {
+        q1 = 0.0f;
+    }
+    if (q2 < 0.0f) {
+        q2 = 0.0f;
+    }
+    if (q3 < 0.0f) {
+        q3 = 0.0f;
+    }
+    q0 = sqrt(q0);
+    q1 = sqrt(q1);
+    q2 = sqrt(q2);
+    q3 = sqrt(q3);
+    if (q0 >= q1 && q0 >= q2 && q0 >= q3) {
+        q0 *= +1.0f;
+        q1 *= SIGN(r32 - r23);
+        q2 *= SIGN(r13 - r31);
+        q3 *= SIGN(r21 - r12);
+    } else if (q1 >= q0 && q1 >= q2 && q1 >= q3) {
+        q0 *= SIGN(r32 - r23);
+        q1 *= +1.0f;
+        q2 *= SIGN(r21 + r12);
+        q3 *= SIGN(r13 + r31);
+    } else if (q2 >= q0 && q2 >= q1 && q2 >= q3) {
+        q0 *= SIGN(r13 - r31);
+        q1 *= SIGN(r21 + r12);
+        q2 *= +1.0f;
+        q3 *= SIGN(r32 + r23);
+    } else if (q3 >= q0 && q3 >= q1 && q3 >= q2) {
+        q0 *= SIGN(r21 - r12);
+        q1 *= SIGN(r31 + r13);
+        q2 *= SIGN(r32 + r23);
+        q3 *= +1.0f;
+    }
+
+    dspm::Mat res(4, 1);
+    res(0, 0) = q0;
+    res(1, 0) = q1;
+    res(2, 0) = q2;
+    res(3, 0) = q3;
+    res /= res.norm();
+    return res;
+}
+
+dspm::Mat ekf::dFdq(dspm::Mat &vector, dspm::Mat &q)
+{
+    dspm::Mat result(3, 4);
+    result(0, 0) = q.data[0] * vector.data[0] - q.data[3] * vector.data[1] + q.data[2] * vector.data[2];
+    result(0, 1) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
+    result(0, 2) = -q.data[2] * vector.data[0] + q.data[1] * vector.data[1] + q.data[0] * vector.data[2];
+    result(0, 3) = -q.data[3] * vector.data[0] - q.data[0] * vector.data[1] + q.data[1] * vector.data[2];
+
+    result(1, 0) = q.data[3] * vector.data[0] + q.data[0] * vector.data[1] - q.data[1] * vector.data[2];
+    result(1, 1) = q.data[2] * vector.data[0] - q.data[1] * vector.data[1] - q.data[0] * vector.data[2];
+    result(1, 2) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
+    result(1, 3) = q.data[0] * vector.data[0] - q.data[3] * vector.data[1] + q.data[2] * vector.data[2];
+
+    result(2, 0) = -q.data[2] * vector.data[0] + q.data[1] * vector.data[1] + q.data[0] * vector.data[2];
+    result(2, 1) = q.data[3] * vector.data[0] + q.data[0] * vector.data[1] - q.data[1] * vector.data[2];
+    result(2, 2) = -q.data[0] * vector.data[0] + q.data[3] * vector.data[1] - q.data[2] * vector.data[2];
+    result(2, 3) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
+
+    result *= 2;
+    return result;
+}
+
+dspm::Mat ekf::dFdq_inv(dspm::Mat &vector, dspm::Mat &q)
+{
+    dspm::Mat result(3, 4);
+    result(0, 0) = q.data[0] * vector.data[0] + q.data[3] * vector.data[1] - q.data[2] * vector.data[2];
+    result(0, 1) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
+    result(0, 2) = -q.data[2] * vector.data[0] + q.data[1] * vector.data[1] - q.data[0] * vector.data[2];
+    result(0, 3) = -q.data[3] * vector.data[0] + q.data[0] * vector.data[1] + q.data[1] * vector.data[2];
+
+    result(1, 0) = -q.data[3] * vector.data[0] + q.data[0] * vector.data[1] + q.data[1] * vector.data[2];
+    result(1, 1) = q.data[2] * vector.data[0] - q.data[1] * vector.data[1] + q.data[0] * vector.data[2];
+    result(1, 2) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
+    result(1, 3) = -q.data[0] * vector.data[0] - q.data[3] * vector.data[1] + q.data[2] * vector.data[2];
+
+    result(2, 0) = q.data[2] * vector.data[0] - q.data[1] * vector.data[1] + q.data[0] * vector.data[2];
+    result(2, 1) = q.data[3] * vector.data[0] - q.data[0] * vector.data[1] - q.data[1] * vector.data[2];
+    result(2, 2) = q.data[0] * vector.data[0] + q.data[3] * vector.data[1] - q.data[2] * vector.data[2];
+    result(2, 3) = q.data[1] * vector.data[0] + q.data[2] * vector.data[1] + q.data[3] * vector.data[2];
+
+    result *= 2;
+    return result;
+}
+
+dspm::Mat ekf::StateXdot(dspm::Mat &x, float *u)
+{
+    dspm::Mat U(u, this->G.cols, 1);
+    dspm::Mat Xdot = (this->F * x + this->G * U);
+    return Xdot;
+}

managed_components/espressif__esp-dsp/modules/kalman/ekf/include/ekf.h

@@ -0,0 +1,255 @@
+// 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>
+
+/**
+ * The ekf is a base class for Extended Kalman Filter.
+ * It contains main matrix operations and define the processing flow.
+ */
+class ekf {
+public:
+
+    /**
+     * Constructor of EKF.
+     * THe constructor allocate main memory for the matrixes.
+     * @param[in] x: - amount of states in EKF. x[n] = F*x[n-1] + G*u + W. Size of matrix F
+     * @param[in] w: - amount of control measurements and noise inputs. Size of matrix G
+    */
+    ekf(int x, int w);
+
+
+    /**
+     * Distructor of EKF
+    */
+    virtual ~ekf();
+    /**
+     * Main processing method of the EKF.
+     *
+     * @param[in] u: - input measurements
+     * @param[in] dt: - time difference from the last call in seconds
+    */
+    virtual void Process(float *u, float dt);
+
+
+    /**
+     * Initialization of EKF.
+     * The method should be called befare the first use of the filter.
+    */
+    virtual void Init() = 0;
+    /**
+     * xDot[n] = F*x[n] + G*u + W
+     * Number of states, X is the state vector (size of F matrix)
+    */
+    int NUMX;
+    /**
+     * xDot[n] = F*x[n] + G*u + W
+     * The size of G matrix
+    */
+    int NUMW;
+
+    /**
+     * System state vector
+    */
+    dspm::Mat &X;
+
+    /**
+     * Linearized system matrices F, where xDot[n] = F*x[n] + G*u + W
+    */
+    dspm::Mat &F;
+    /**
+     * Linearized system matrices G, where xDot[n] = F*x[n] + G*u + W
+    */
+    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
+     *
+     * @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
+     * The default calculation: xDot = F*x + G*u
+     * It's possible to implement optimized version
+     * @param[in] x: state vector
+     * @param[in] u: control measurement
+     * @return
+     *      xDot - derivative of input vector x and u
+     */
+    virtual dspm::Mat StateXdot(dspm::Mat &x, float *u);
+    /**
+     * 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);
+
+    /**
+     * Matrix for intermidieve calculations
+    */
+    float *HP;
+    /**
+     * Matrix for intermidieve calculations
+    */
+    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] q: 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_

managed_components/espressif__esp-dsp/modules/kalman/ekf_imu13states/docs/README.md

@@ -0,0 +1,38 @@
+# Extended Kalman Filter for IMU sensor with 13 calculated states 
+
+The extended Kalman filter (EKF) with 13 states designed to be used with data from common 
+IMU sensor, like MPU9250 or MPU6050. These sensors provide gyroscope, accelerometer 
+and magnetometer data. The sensors have measurement errors, and it's not possible
+to use them for calculation system attitude.
+For that the EKF should be used.
+Current implementation of EKF calculates 13 value to define the system state:
+    1. System attitude [0..3]: quaternion that describes absolute value of system attitude (w, x,y,z)
+    2. Gyroscope sensor errors[4..6]: bias values (x,y,z)
+    3. Magnetometer vector value[7..9]: absolute value of magnetometer sensor (x,y,z)
+    4. Magnetometer vector offset[10..12]: absolute value of magnetometer sensor offset (x,y,z)
+These values calculated by the EKF and stored to the X vector.
+Magnetometer value from the sensor has some value and offset. This information needed to calculate system attitude.
+Magnetometer sensor value = R*Magn_Amplitude + Magn_offset. 
+Where  R - rotation matrix from system attitude,
+Magn_Amplitude - real magnetometer value calculated by the EKF
+Magn_offset - magnetometer offset value (called deviation)
+
+
+## How to use EKF
+The EKF has two main methods: state calculation (Process(...)) and state correction (UpdateRefMeasurement() in our case).
+The current EKF has one main input value - is a gyroscope (gyro) angular speed. If the gyro has no bias error, then it's enough to call Process(...) method, and EKF 
+will calculate system attitude. But, the gyro has a bias error and that's why, the UpdateRefMeasurement(...) must be called, when reference accelerometer and magnetometer values are available.
+After the firs start, the EKF will need some time to calculate correct gyro bias and magnetometer deviation. This is a calibration phase. 
+To avoid this phase every time after filter started, it's better to store X vector and P matrix to the non-volatile memory, and restore them after system started.
+The sequence in this case will be:
+Init()-> restore X and P values (if exist) -> go to normal process 
+
+
+## Adjustable parameters
+There two lists of parameters that could be adjusted: R - measurement noise covariance values, and Q - model noise matrix.
+The R is an array with values for diagonal matrix. Then smaller value R for reference value, then more EKF will trust for this value.
+In normal case, the R values should be between 0.001 and 1.
+The Q - is a diagonal (only values in main diagonal are important) noise matrix of state vector. This matrix define how good value in state vector X should be.
+
+
+

managed_components/espressif__esp-dsp/modules/kalman/ekf_imu13states/ekf_imu13states.cpp

@@ -0,0 +1,293 @@
+// 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.
+
+#include "ekf_imu13states.h"
+
+ekf_imu13states::ekf_imu13states() : ekf(13, 18),
+    mag0(3, 1),
+    accel0(3, 1)
+{
+    this->NUMU = 3;
+}
+
+ekf_imu13states::~ekf_imu13states()
+{
+}
+
+void ekf_imu13states::Init()
+{
+    mag0.data[0] = 1;
+    mag0.data[1] = 0;
+    mag0.data[2] = 0;
+
+    mag0 /= mag0.norm();
+
+    accel0.data[0] = 0;
+    accel0.data[1] = 0;
+    accel0.data[2] = 1;
+
+    accel0 /= accel0.norm();
+
+    this->Q.Copy(0.1 * dspm::Mat::eye(3), 0, 0);
+    this->Q.Copy(0.0001 * dspm::Mat::eye(3), 3, 3);
+    this->Q.Copy(0.0001 * dspm::Mat::eye(3), 6, 6);
+    this->Q.Copy(0.0001 * dspm::Mat::eye(3), 9, 9);
+    this->Q.Copy(0.00001 * dspm::Mat::eye(3), 12, 12);
+    this->Q.Copy(0.00001 * dspm::Mat::eye(3), 15, 15);
+
+    this->X.data[0] = 1; // Init quaternion
+    this->X.data[7] = 1; // Initial magnetometer vector
+}
+
+dspm::Mat ekf_imu13states::StateXdot(dspm::Mat &x, float *u)
+{
+    float wx = u[0] - x(4, 0); // subtract the biases on gyros
+    float wy = u[1] - x(5, 0);
+    float wz = u[2] - x(6, 0);
+
+    float w[] = {wx, wy, wz};
+    dspm::Mat q = dspm::Mat(x.data, 4, 1);
+
+    // qdot = Q * w
+    dspm::Mat Omega = 0.5 * SkewSym4x4(w);
+    dspm::Mat qdot = Omega * q;
+    dspm::Mat Xdot(this->NUMX, 1);
+    Xdot *= 0;
+    Xdot.Copy(qdot, 0, 0);
+    // dwbias = 0
+    // dMang_Ampl = 0
+    // dMang_offset = 0
+    return Xdot;
+}
+
+void ekf_imu13states::LinearizeFG(dspm::Mat &x, float *u)
+{
+    float w[3] = {(u[0] - x(4, 0)), (u[1] - x(5, 0)), (u[2] - x(6, 0))}; // subtract the biases on gyros
+    // float w[3] = {u[0], u[1], u[2]}; // subtract the biases on gyros
+
+    this->F *= 0; // Initialize F and G matrixes.
+    this->G *= 0;
+
+    // dqdot / dq - skey matrix
+    F.Copy(0.5 * ekf::SkewSym4x4(w), 0, 0);
+
+    // dqdot/dvector
+    dspm::Mat dq = -0.5 * qProduct(x.data);
+    dspm::Mat dq_q = dq.Get(0, 4, 1, 3);
+
+    // dqdot / dnw
+    G.Copy(dq_q, 0, 0);
+    // dqdot / dwbias
+    F.Copy(dq_q, 0, 4);
+
+    dspm::Mat rotm = -1 * this->quat2rotm(x.data); // Convert quat to rotation matrix
+
+    G.Copy(rotm, 7, 6);
+    G.Copy(dspm::Mat::eye(3), 4, 3);   // random noise wbias
+    G.Copy(dspm::Mat::eye(3), 7, 12);  // random noise magnetometer amplitude
+    G.Copy(dspm::Mat::eye(3), 10, 9);  // magnetometer offset constant
+    G.Copy(dspm::Mat::eye(3), 10, 15); // random noise offset constant
+}
+
+void ekf_imu13states::Test()
+{
+    dspm::Mat test_x(7, 1);
+    for (size_t i = 0; i < 7; i++) {
+        test_x(i, 0) = i;
+    }
+    printf("Allocate data = %i\n", this->NUMU);
+    float *test_u = new float[this->NUMU];
+    for (size_t i = 0; i < this->NUMU; i++) {
+        test_u[i] = i;
+    }
+    dspm::Mat result_StateXdot = StateXdot(test_x, test_u);
+    delete[] test_u;
+}
+
+void ekf_imu13states::TestFull(bool enable_att)
+{
+    int total_N = 2048;
+    float pi = std::atan(1) * 4;
+    float gyro_err_data[] = {0.1, 0.2, 0.3}; // static constatnt error
+    dspm::Mat gyro_err(gyro_err_data, 3, 1);
+    float R[10];
+    for (size_t i = 0; i < 10; i++) {
+        R[i] = 0.01;
+    }
+
+    float accel0_data[] = {0, 0, 1};
+    float magn0_data[] = {1, 0, 0};
+
+    dspm::Mat accel0(accel0_data, 3, 1);
+    dspm::Mat magn0(magn0_data, 3, 1);
+
+    float dt = 0.01;
+
+    dspm::Mat gyro_data(3, 1);
+    int count = 0;
+
+    // Initial rotation matrix
+    dspm::Mat Rm = dspm::Mat::eye(3);
+    dspm::Mat Re = dspm::Mat::eye(3);
+
+    gyro_err *= 1;
+
+    std::cout << "Gyro error: " << gyro_err.t() << std::endl;
+    for (size_t n = 1; n < total_N * 3; n++) {
+        if ((n % 1000) == 0) {
+            std::cout << "Loop " << n << " from " << total_N * 16;
+            std::cout << ", State data : " << this->X.t();
+        }
+        gyro_data *= 0; // reset gyro value
+        if ((n >= (total_N / 2)) && (n < total_N * 12)) {
+            gyro_data(0, 0) = 1 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
+            gyro_data(1, 0) = 2 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
+            gyro_data(2, 0) = 3 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
+            count++;
+        }
+        dspm::Mat gyro_sample = gyro_data + gyro_err;
+
+        gyro_data *= dt;
+        Re = this->eul2rotm(gyro_data.data); // Calculate rotation to gyro angel
+        Rm = Rm * Re;                        // Rotate original matrix
+        dspm::Mat attitude = ekf::rotm2quat(Rm);
+        // We have to rotate accel and magn to the opposite direction
+        dspm::Mat accel_data = Rm.t() * accel0;
+        dspm::Mat magn_data = Rm.t() * magn0;
+
+        dspm::Mat accel_norm = accel_data / accel_data.norm();
+        dspm::Mat magn_norm = magn_data / magn_data.norm();
+
+        float input_u[] = {gyro_sample(0, 0), gyro_sample(1, 0), gyro_sample(2, 0)};
+        // Process input values to new state
+        this->Process(input_u, dt);
+        dspm::Mat q_norm(this->X.data, 4, 1);
+        q_norm /= q_norm.norm();
+
+        if (true == enable_att) {
+            this->UpdateRefMeasurement(accel_norm.data, magn_norm.data, attitude.data, R);
+        } else {
+            this->UpdateRefMeasurement(accel_norm.data, magn_norm.data, R);
+        }
+    }
+    std::cout << "Final State data : " << this->X.t() << std::endl;
+}
+
+void ekf_imu13states::UpdateRefMeasurement(float *accel_data, float *magn_data, float R[6])
+{
+    dspm::Mat quat(this->X.data, 4, 1);
+    dspm::Mat H = 0 * dspm::Mat(6, this->NUMX);
+    dspm::Mat Re = this->quat2rotm(quat.data).t();
+
+    // dAccel/dq
+    dspm::Mat dAccel_dq = ekf::dFdq_inv(this->accel0, quat);
+    H.Copy(dAccel_dq, 3, 0);
+
+    // dMagn/dq
+    dspm::Mat magn(&this->X.data[7], 3, 1);
+    dspm::Mat magn_offset(&this->X.data[10], 3, 1);
+    dspm::Mat dMagn_dq = ekf::dFdq_inv(magn, quat);
+    H.Copy(dMagn_dq, 0, 0);
+
+    dspm::Mat expected_magn = Re * magn + magn_offset;
+    dspm::Mat expected_accel = Re * this->accel0;
+
+    float measured_data[6];
+    float expected_data[6];
+    for (size_t i = 0; i < 3; i++) {
+        measured_data[i] = magn_data[i];
+        expected_data[i] = expected_magn.data[i];
+        measured_data[i + 3] = accel_data[i];
+        expected_data[i + 3] = expected_accel.data[i];
+    }
+
+    this->Update(H, measured_data, expected_data, R);
+    quat /= quat.norm();
+}
+
+void ekf_imu13states::UpdateRefMeasurementMagn(float *accel_data, float *magn_data, float R[6])
+{
+    dspm::Mat quat(this->X.data, 4, 1);
+    dspm::Mat H = 0 * dspm::Mat(6, this->NUMX);
+    dspm::Mat Re = this->quat2rotm(quat.data).t();
+
+    // We include these two line to update magnetometer initial state
+    H.Copy(Re, 0, 7);
+    H.Copy(dspm::Mat::eye(3), 0, 10);
+
+    // dAccel/dq
+    dspm::Mat dAccel_dq = ekf::dFdq_inv(this->accel0, quat);
+    H.Copy(dAccel_dq, 3, 0);
+
+    // dMagn/dq
+    dspm::Mat magn(&this->X.data[7], 3, 1);
+    dspm::Mat magn_offset(&this->X.data[10], 3, 1);
+    dspm::Mat dMagn_dq = ekf::dFdq_inv(magn, quat);
+    H.Copy(dMagn_dq, 0, 0);
+
+    dspm::Mat expected_magn = Re * magn + magn_offset;
+    dspm::Mat expected_accel = Re * this->accel0;
+
+    float measured_data[6];
+    float expected_data[6];
+    for (size_t i = 0; i < 3; i++) {
+        measured_data[i] = magn_data[i];
+        expected_data[i] = expected_magn.data[i];
+        measured_data[i + 3] = accel_data[i];
+        expected_data[i + 3] = expected_accel.data[i];
+    }
+
+    this->Update(H, measured_data, expected_data, R);
+    quat /= quat.norm();
+}
+
+void ekf_imu13states::UpdateRefMeasurement(float *accel_data, float *magn_data, float *attitude, float R[10])
+{
+    dspm::Mat quat(this->X.data, 4, 1);
+    dspm::Mat H = 0 * dspm::Mat(10, this->NUMX);
+    dspm::Mat Re = this->quat2rotm(quat.data).t();
+
+    H.Copy(Re, 0, 7);
+    H.Copy(dspm::Mat::eye(3), 0, 10);
+    // dAccel/dq
+    dspm::Mat dAccel_dq = ekf::dFdq_inv(this->accel0, quat);
+    H.Copy(dAccel_dq, 3, 0);
+    // dMagn/dq
+    dspm::Mat magn(&this->X.data[7], 3, 1);
+    dspm::Mat magn_offset(&this->X.data[10], 3, 1);
+    dspm::Mat dMagn_dq = ekf::dFdq_inv(magn, quat);
+    H.Copy(dMagn_dq, 0, 0);
+
+    // dq/dq
+    H.Copy(dspm::Mat::eye(4), 6, 1);
+
+    dspm::Mat expected_magn = Re * magn + magn_offset;
+    dspm::Mat expected_accel = Re * this->accel0;
+
+    float measured_data[10];
+    float expected_data[10];
+    for (size_t i = 0; i < 3; i++) {
+        measured_data[i] = magn_data[i];
+        expected_data[i] = expected_magn.data[i];
+        measured_data[i + 3] = accel_data[i];
+        expected_data[i + 3] = expected_accel.data[i];
+    }
+    for (size_t i = 0; i < 4; i++) {
+        measured_data[i + 6] = attitude[i];
+        expected_data[i + 6] = this->X.data[i];
+    }
+
+    this->Update(H, measured_data, expected_data, R);
+    quat /= quat.norm();
+}

managed_components/espressif__esp-dsp/modules/kalman/ekf_imu13states/include/ekf_imu13states.h

@@ -0,0 +1,98 @@
+// 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);
+
+    /**
+    *     Method for development and tests only.
+    */
+    void Test();
+    /**
+    *     Method for development and tests only.
+    *
+    * @param[in] enable_att - enable attitude as input reference value
+    */
+    void TestFull(bool enable_att);
+
+    /**
+    *     Initial reference valie for magnetometer.
+    */
+    dspm::Mat mag0;
+    /**
+    *     Initial reference valie for accelerometer.
+    */
+    dspm::Mat accel0;
+
+    /**
+    * number of control measurements
+    */
+    int NUMU;
+
+    /**
+     * 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_

managed_components/espressif__esp-dsp/modules/kalman/ekf_imu13states/test/test_ekf_imu13states.cpp

@@ -0,0 +1,61 @@
+// Copyright 2018-2019 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.
+
+#include <string.h>
+#include "unity.h"
+#include "dsp_platform.h"
+#include "esp_log.h"
+
+#include "dsp_tests.h"
+#include "ekf_imu13states.h"
+#include "esp_attr.h"
+
+static const char *TAG = "ekf_imu13states";
+
+
+TEST_CASE("ekf_imu13states functionality gyro only", "[dspm]")
+{
+    ekf_imu13states *ekf13 = new  ekf_imu13states();
+    ekf13->Init();
+    ekf13->Test();
+    unsigned int start_b = dsp_get_cpu_cycle_count();
+    ekf13->TestFull(false);
+    unsigned int end_b = dsp_get_cpu_cycle_count();
+    ESP_LOGI(TAG, "Total time %i (K cycles)", (end_b - start_b) / 1000);
+    TEST_ASSERT_LESS_THAN(100, (int)(1000 * abs(ekf13->X.data[4] - 0.1)));
+    TEST_ASSERT_LESS_THAN(100, (int)(1000 * abs(ekf13->X.data[5] - 0.2)));
+    TEST_ASSERT_LESS_THAN(100, (int)(1000 * abs(ekf13->X.data[6] - 0.3)));
+    printf("Expected result = %i, calculated result = %i\n", 100, (int)(1000 * ekf13->X.data[4] + 0.5));
+    printf("Expected result = %i, calculated result = %i\n", 200, (int)(1000 * ekf13->X.data[5] + 0.5));
+    printf("Expected result = %i, calculated result = %i\n", 300, (int)(1000 * ekf13->X.data[6] + 0.5));
+
+}
+
+TEST_CASE("ekf_imu13states functionality gyro and magn", "[dspm]")
+{
+    ekf_imu13states *ekf13 = new  ekf_imu13states();
+    ekf13->Init();
+    ekf13->Test();
+    unsigned int start_b = dsp_get_cpu_cycle_count();
+    ekf13->TestFull(true);
+    unsigned int end_b = dsp_get_cpu_cycle_count();
+    ESP_LOGI(TAG, "Total time %i (K cycles)", (end_b - start_b) / 1000);
+
+    TEST_ASSERT_LESS_THAN(600, (int)(1000 * abs(ekf13->X.data[4] - 0.1)));
+    TEST_ASSERT_LESS_THAN(600, (int)(1000 * abs(ekf13->X.data[5] - 0.2)));
+    TEST_ASSERT_LESS_THAN(600, (int)(1000 * abs(ekf13->X.data[6] - 0.3)));
+    printf("Expected result = %i, calculated result = %i\n", 100, (int)(1000 * ekf13->X.data[4] + 0.5));
+    printf("Expected result = %i, calculated result = %i\n", 200, (int)(1000 * ekf13->X.data[5] + 0.5));
+    printf("Expected result = %i, calculated result = %i\n", 300, (int)(1000 * ekf13->X.data[6] + 0.5));
+}