srcf_core.hpp

Go to the documentation of this file.

// Copyright 2018 the Autoware Foundation
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
#ifndef KALMAN_FILTER__SRCF_CORE_HPP_
#define KALMAN_FILTER__SRCF_CORE_HPP_

#include <common/types.hpp>
#include <kalman_filter/visibility_control.hpp>
#include <Eigen/Core>

#include <cmath>
#include <limits>

using autoware::common::types::float32_t;

namespace autoware
{
namespace prediction
{
namespace kalman_filter
{
using index_t = Eigen::Index;

template<int32_t NumStates, int32_t ProcessNoiseDim>
class SrcfCore
{
public:
  using state_vec_t = Eigen::Matrix<float32_t, NumStates, 1>;
  using square_mat_t = Eigen::Matrix<float32_t, NumStates, NumStates>;

  template<int32_t ColN1, int32_t ColN2>
  static void zero_row(
    Eigen::Matrix<float32_t, NumStates, ColN1> & A,
    Eigen::Matrix<float32_t, NumStates, ColN2> & B,
    const index_t row,
    const index_t col_start,
    const index_t col_end)
  {
    // TODO(cvasfi): Use the proper Eigen API (eg. JacobiRotation) for matrix decompositions

    for (index_t j = col_start; j < col_end; ++j) {
      const float32_t g = B(row, j);
      // TODO(c.ho) benchmark branchi-ness
      if (fabsf(g) <= std::numeric_limits<decltype(g)>::epsilon()) {
        // c = 1, s = 0
        // Do nothing, identity rotation
      } else if (fabsf(A(row, row)) <= std::numeric_limits<decltype(g)>::epsilon()) {
        // c = 0, s = sign(g)
        for (index_t k = row; k < NumStates; ++k) {
          const float32_t tau = std::copysign(A(k, row), g);
          A(k, row) = std::copysign(B(k, j), g);
          B(k, j) = tau;
        }
      } else {
        const float32_t f = A(row, row);
        const float32_t ir = 1.0F / std::copysign(sqrtf((f * f) + (g * g)), f);
        const float32_t s = g * ir;
        const float32_t c = fabsf(f * ir);
        // TODO(c.ho) benchmark BLAS
        // probably slightly more efficient to do it this way than ~6 ish BLAS ops
        for (index_t k = row; k < NumStates; ++k) {
          const float32_t tau = (c * B(k, j)) - (s * A(k, row));
          A(k, row) = (s * B(k, j)) + (c * A(k, row));
          B(k, j) = tau;
        }
      }
    }
  }

  template<int32_t NCols2>
  static void right_lower_triangularize_matrices(
    square_mat_t & A,
    Eigen::Matrix<float32_t, NumStates, NCols2> & B,
    const index_t b_rank = NCols2)
  {
    // For each row in fat partitioned matrix, [A | B]
    for (index_t i = index_t{}; i < NumStates; ++i) {
      zero_row(A, B, i, index_t(), b_rank);
      zero_row(A, A, i, i + 1, NumStates);
    }
  }

  template<typename Derived>
  float32_t scalar_update(
    const float32_t z,
    const float32_t r,
    const Eigen::MatrixBase<Derived> & h,
    square_mat_t & C,
    state_vec_t & x)
  {
    // This implementation is adapted from the lower triangle update in
    // "Estimation with Applications to Tracking and Navigation" Bar-shalom & Li,
    // Ch 7.4: pg314-315
    // Where the implementation is modified by applying the diagonal scaling in-place,
    // i.e. P = LDL^T = SS^T <--> S = LD^(1/2)
    // all operations are guanteed to be in domain if r is positive
    if (r <= 0.0F) {
      throw std::domain_error("ScrfCore: measurement noise variance must be positive");
    }
    float32_t alpha = r;
    m_k.setZero();
    for (index_t idx = NumStates - 1; idx < NumStates && idx >= 0; --idx) {
      const index_t jdx = NumStates - idx;
      Eigen::Block<square_mat_t> col = C.block(idx, idx, jdx, 1);
      // f_j = C_j^T * h^T, C_j = j'th col of C
      const float32_t sigma =
        (h.block(0, idx, 1, jdx) * col)(0, 0);
      // beta = alpha_{j-1}
      const float32_t beta = alpha;
      // alpha_j = alpha_{j-1} + (f_j^2)
      alpha += (sigma * sigma);
      m_tau.block(0, 0, jdx, 1) = col;
      const float32_t zetap = -sigma / beta;  // beta is positive if R is positive
      // Update covariance column C_j = -K_{j-1} * (f_j / beta) + C_j
      col += zetap * m_k.block(idx, 0, jdx, 1);
      const float32_t etap = sqrtf(beta / alpha);  // alpha positive if beta is
      // scale by diagonal factor C_j = C_j * sqrt(beta / alpha)
      col *= etap;
      // accumulate unscaled kalman gain: K = K + f_j * C_{j, -}
      m_k.block(idx, 0, jdx, 1) += sigma * m_tau.block(0, 0, jdx, 1);
    }
    const float32_t del = z - (h * x)(0, 0);

    // alpha established to be positive
    if (alpha <= 0.0F) {
      // if code is written properly, this branch should never hit!
      throw std::runtime_error("ScrfCore: invariant broken, kalman gain must be positive");
    }
    // Update state vector by error * unscaled kalman_gain
    const float32_t del_alpha = del / alpha;
    x += del_alpha * m_k;

    // constant for computation (logf isn't constexpr due to errno)
    constexpr float32_t logf2pi = 1.83787706641F;
    // alpha = r + h^T * P * h = s = scalar component of innovation covariance (variance)
    // use gaussian likelihood
    return -0.5F * (logf2pi + logf(alpha) + (del * del_alpha));
  }

private:
  state_vec_t m_tau;
  state_vec_t m_k;
  static_assert(NumStates > 0U, "must have positive number of states");
  static_assert(ProcessNoiseDim > 0U, "must have positive number of process noise dimensions");
};  // class SrcfCore
}  // namespace kalman_filter
}  // namespace prediction
}  // namespace autoware

#endif  // KALMAN_FILTER__SRCF_CORE_HPP_