This class implements the partially norm-constrained linear Kalman filter [5]. More...

#include <nckf.h>

Inheritance diagram for mrs_lib::NCLKF_partial< n_states, n_inputs, n_measurements, n_norm_constrained_states >:

Collaboration diagram for mrs_lib::NCLKF_partial< n_states, n_inputs, n_measurements, n_norm_constrained_states >:

Public Types
using	Base_class = NCLKF< n, m, p >
	Base class of this class.

using	x_t = typename Base_class::x_t
	State vector type $n \times 1$ .

using	u_t = typename Base_class::u_t
	Input vector type $m \times 1$ .

using	z_t = typename Base_class::z_t
	Measurement vector type $p \times 1$ .

using	P_t = typename Base_class::P_t
	State uncertainty covariance matrix type $n \times n$ .

using	R_t = typename Base_class::R_t
	Measurement noise covariance matrix type $p \times p$ .

using	Q_t = typename Base_class::Q_t
	Process noise covariance matrix type $n \times n$ .

using	statecov_t = typename Base_class::statecov_t
	Helper struct for passing around the state and its covariance in one variable.

using	A_t = typename Base_class::A_t
	System transition matrix type $n \times n$ .

using	B_t = typename Base_class::B_t
	Input to state mapping matrix type $n \times m$ .

using	H_t = typename Base_class::H_t
	State to measurement mapping matrix type $p \times n$ .

using	K_t = typename Base_class::K_t
	Kalman gain matrix type $n \times p$ .

using	indices_t = std::array< int, nq >
	Indices of the norm-constrained states type.

using	xq_t = Eigen::Matrix< double, nq, 1 >
	Norm-constrained states vector type $n_q \times 1$ .

using	Hq_t = Eigen::Matrix< double, p, nq >
	Norm-constrained measurement mapping type $p \times n_q$ .

using	Kq_t = Eigen::Matrix< double, nq, p >
	Norm-constrained kalman gain type $n_q \times p$ .

Public Types inherited from mrs_lib::NCLKF< n_states, n_inputs, n_measurements >
using	Base_class = LKF< n, m, p >
	Base class of this class.

using	x_t = typename Base_class::x_t
	State vector type $n \times 1$ .

using	u_t = typename Base_class::u_t
	Input vector type $m \times 1$ .

using	z_t = typename Base_class::z_t
	Measurement vector type $p \times 1$ .

using	P_t = typename Base_class::P_t
	State uncertainty covariance matrix type $n \times n$ .

using	R_t = typename Base_class::R_t
	Measurement noise covariance matrix type $p \times p$ .

using	Q_t = typename Base_class::Q_t
	Process noise covariance matrix type $n \times n$ .

using	statecov_t = typename Base_class::statecov_t
	Helper struct for passing around the state and its covariance in one variable.

using	A_t = typename Base_class::A_t
	System transition matrix type $n \times n$ .

using	B_t = typename Base_class::B_t
	Input to state mapping matrix type $n \times m$ .

using	H_t = typename Base_class::H_t
	State to measurement mapping matrix type $p \times n$ .

using	K_t = typename Base_class::K_t
	Kalman gain matrix type $n \times p$ .

Public Types inherited from mrs_lib::LKF< n_states, n_inputs, n_measurements >
using	Base_class = KalmanFilter< n, m, p >
	Base class of this class.

using	x_t = typename Base_class::x_t
	State vector type $n \times 1$ .

using	u_t = typename Base_class::u_t
	Input vector type $m \times 1$ .

using	z_t = typename Base_class::z_t
	Measurement vector type $p \times 1$ .

using	P_t = typename Base_class::P_t
	State uncertainty covariance matrix type $n \times n$ .

using	R_t = typename Base_class::R_t
	Measurement noise covariance matrix type $p \times p$ .

using	Q_t = typename Base_class::Q_t
	Process noise covariance matrix type $n \times n$ .

using	statecov_t = typename Base_class::statecov_t
	Helper struct for passing around the state and its covariance in one variable.

typedef Eigen::Matrix< double, n, n >	A_t
	System transition matrix type $n \times n$ .

typedef Eigen::Matrix< double, n, m >	B_t
	Input to state mapping matrix type $n \times m$ .

typedef Eigen::Matrix< double, p, n >	H_t
	State to measurement mapping matrix type $p \times n$ .

typedef Eigen::Matrix< double, n, p >	K_t
	Kalman gain matrix type $n \times p$ .

Public Types inherited from mrs_lib::KalmanFilter< n_states, n_inputs, n_measurements >
typedef Eigen::Matrix< double, n, 1 >	x_t
	State vector type $n \times 1$ .

typedef Eigen::Matrix< double, m, 1 >	u_t
	Input vector type $m \times 1$ .

typedef Eigen::Matrix< double, p, 1 >	z_t
	Measurement vector type $p \times 1$ .

typedef Eigen::Matrix< double, n, n >	P_t
	State uncertainty covariance matrix type $n \times n$ .

typedef Eigen::Matrix< double, p, p >	R_t
	Measurement noise covariance matrix type $p \times p$ .

typedef Eigen::Matrix< double, n, n >	Q_t
	Process noise covariance matrix type $n \times n$ .

Public Member Functions
	NCLKF_partial ()
	Convenience default constructor. More...

	NCLKF_partial (const A_t &A, const B_t &B, const H_t &H, const double l, const indices_t &norm_constrained_indices)
	The main constructor. More...

Public Member Functions inherited from mrs_lib::NCLKF< n_states, n_inputs, n_measurements >
	NCLKF ()
	Convenience default constructor. More...

	NCLKF (const A_t &A, const B_t &B, const H_t &H, const double l)
	The main constructor. More...

Public Member Functions inherited from mrs_lib::LKF< n_states, n_inputs, n_measurements >
	LKF ()
	Convenience default constructor. More...

	LKF (const A_t &A, const B_t &B, const H_t &H)
	The main constructor. More...

virtual statecov_t	correct (const statecov_t &sc, const z_t &z, const R_t &R) const override
	Applies the correction (update, measurement, data) step of the Kalman filter. More...

virtual statecov_t	predict (const statecov_t &sc, const u_t &u, const Q_t &Q, [[maybe_unused]] double dt) const override
	Applies the prediction (time) step of the Kalman filter. More...

Public Member Functions inherited from mrs_lib::KalmanFilter< n_states, n_inputs, n_measurements >
virtual statecov_t	correct (const statecov_t &sc, const z_t &z, const R_t &R) const =0
	Applies the correction (update, measurement, data) step of the Kalman filter. More...

virtual statecov_t	predict (const statecov_t &sc, const u_t &u, const Q_t &Q, double dt) const =0
	Applies the prediction (time) step of the Kalman filter. More...

Static Public Attributes
static const int	n = n_states
	Length of the state vector of the system.

static const int	m = n_inputs
	Length of the input vector of the system.

static const int	p = n_measurements
	Length of the measurement vector of the system.

static const int	nq = n_norm_constrained_states
	Number of states to which the norm constraint applies.

Static Public Attributes inherited from mrs_lib::NCLKF< n_states, n_inputs, n_measurements >
static const int	n = n_states
	Length of the state vector of the system.

static const int	m = n_inputs
	Length of the input vector of the system.

static const int	p = n_measurements
	Length of the measurement vector of the system.

Static Public Attributes inherited from mrs_lib::LKF< n_states, n_inputs, n_measurements >
static constexpr int	n = n_states
	Length of the state vector of the system.

static constexpr int	m = n_inputs
	Length of the input vector of the system.

static constexpr int	p = n_measurements
	Length of the measurement vector of the system.

Static Public Attributes inherited from mrs_lib::KalmanFilter< n_states, n_inputs, n_measurements >
static const int	n = n_states
	Length of the state vector of the system.

static const int	m = n_inputs
	Length of the input vector of the system.

static const int	p = n_measurements
	Length of the measurement vector of the system.

Protected Member Functions
virtual K_t	computeKalmanGain (const statecov_t &sc, const z_t &z, const R_t &R, const H_t &H) const override

Protected Member Functions inherited from mrs_lib::LKF< n_states, n_inputs, n_measurements >
virtual K_t	computeKalmanGain (const statecov_t &sc, [[maybe_unused]] const z_t &z, const R_t &R, const H_t &H) const

statecov_t ::type	correction_impl (const statecov_t &sc, const z_t &z, const R_t &R, const H_t &H) const

Additional Inherited Members
Public Attributes inherited from mrs_lib::LKF< n_states, n_inputs, n_measurements >
A_t	A
	The system transition matrix $n \times n$ .

B_t	B
	The input to state mapping matrix $n \times m$ .

H_t	H
	The state to measurement mapping matrix $p \times n$ .

Static Protected Member Functions inherited from mrs_lib::LKF< n_states, n_inputs, n_measurements >
static P_t	covariance_predict (const A_t &A, const P_t &P, const Q_t &Q, const double dt)

template<int check = n_inputs>
static std::enable_if< check==0, x_t >::type	state_predict (const A_t &A, const x_t &x, [[maybe_unused]] const B_t &B, [[maybe_unused]] const u_t &u)

template<int check = n_inputs>
static std::enable_if< check !=0, x_t >::type	state_predict (const A_t &A, const x_t &x, const B_t &B, const u_t &u)

static R_t	invert_W (R_t W)

Protected Attributes inherited from mrs_lib::NCLKF< n_states, n_inputs, n_measurements >
double	l

Detailed Description

template<int n_states, int n_inputs, int n_measurements, int n_norm_constrained_states>
class mrs_lib::NCLKF_partial< n_states, n_inputs, n_measurements, n_norm_constrained_states >

This class implements the partially norm-constrained linear Kalman filter [5].

Note that the class is templated. The template parameters specify the number of states, number of inputs and number of measurements of the system. The last template parameter specifies the number of states which are norm-constrained.

The norm constraint is specified in the constructor together with the indices of the states to which the constraint applies.

Example usage:

// clang: MatousFormat
 
#include <mrs_lib/nckf.h>
#include <random>
 
namespace mrs_lib
{
  const int n_states = -1;
  const int n_states_norm_constrained = 2;
  const int n_inputs = 1;
  const int n_measurements = 4;
 
  /* using ukf_t = NCUKF<n_states, n_inputs, n_measurements>; */
  using lkf_t = NCLKF_partial<n_states, n_inputs, n_measurements, n_states_norm_constrained>;
}
 
constexpr int n = 4;
 
using namespace mrs_lib;
using Q_t = lkf_t::Q_t;
using x_t = lkf_t::x_t;
using P_t = lkf_t::P_t;
using u_t = lkf_t::u_t;
using z_t = lkf_t::z_t;
using R_t = lkf_t::R_t;
 
using A_t = lkf_t::A_t;
using B_t = lkf_t::B_t;
using H_t = lkf_t::H_t;
 
A_t A;
B_t B;
H_t H;
 
 
// This function implements the state transition
x_t tra_model_f(const x_t& x, const u_t& u, [[maybe_unused]] const double dt)
{
  x_t ret;
  ret = A*x + B*u;
  auto xq = ret.block<2, 1>(0, 0);
  xq = xq/xq.norm();
  ret.block<2, 1>(2, 0) = xq;
  return ret;
}
 
// This function implements the observation generation from a state
z_t obs_model_f(const x_t& x)
{
  return H*x;
}
 
template <int rows>
Eigen::Matrix<double, rows, 1> normal_randmat(const Eigen::Matrix<double, rows, rows>& cov)
{
    static std::random_device rd{};
    static std::mt19937 gen{rd()};
    static std::normal_distribution<> d{0,1};
    Eigen::Matrix<double, rows, 1> ret(n, 1);
    for (int row = 0; row < rows; row++)
      ret(row, 0) = d(gen);
    return cov*ret;
}
 
int main()
{
  srand(std::time(0));
  const double l = 3.0;
 
  // dt will be constant in this example
  const double dt = 1.0;
 
  // Initialize the process noise matrix
  Q_t Q(n,n); Q << 
    1e-3, 0, 0, 0,
    0, 1e-3, 0, 0,
    0, 0, 1e-2, 0,
    0, 0, 0, 1e-2;
 
  // Initialize the measurement noise matrix
  R_t R = 1e-2 * R_t::Ones();
 
  // Initialize the state transition matrix
  A = A_t(n,n);
  A << 
    1, dt, 0.5*dt*dt, 0.25*dt*dt*dt,
    0, 1, dt, 0.5*dt*dt,
    0, 0, 1, dt,
    0, 0, 0, 0.9;
 
  // Initialize the input matrix
  B = B_t(n, n_inputs);
  B << 
    0,
    0,
    0,
    1;
 
  // Initialize the observation matrix
  H = H_t(n_measurements, n);
  H <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1;
 
  // Generate initial state and covariance
  x_t x0 = 100.0*x_t::Random(n);
  x0(2) = 0.0;
  x0(3) = 10.0;
  x0 = x0/x0.norm();
  P_t P_tmp = P_t::Random(n,n);
  const P_t P0 = 10.0*P_tmp*P_tmp.transpose();
  const lkf_t::statecov_t sc0({x0, P0});
 
  const lkf_t::indices_t nconst_idxs = {0,1}; // these are the norm-constrained states
  lkf_t lkf(A, B, H, l, nconst_idxs);
 
  const int n_its = 1e1;
  std::vector<lkf_t::statecov_t> lscs;
  std::vector<lkf_t::statecov_t> scs;
 
  lscs.reserve(n_its+1);
  scs.reserve(n_its+1);
 
  lscs.push_back(sc0);
  scs.push_back(sc0);
 
  for (int it = 0; it < n_its; it++)
  {
    std::cout << "step: " << it << std::endl;
 
    // Generate a new input vector
    /* const u_t u = u_t(0.5) + 0.1*u_t::Random(); */
    const u_t u = u_t(0.0);
    
    // Generate a new state according to the model and noise parameters
    auto sc = scs.back();
    sc.x = tra_model_f(sc.x, u, dt) + normal_randmat(Q);
    sc.x = sc.x / sc.x.norm();
    
    // Generate a new observation according to the model and noise parameters
    const z_t z = obs_model_f(sc.x) + normal_randmat(R);
 
    scs.push_back(sc);
    scs.push_back(sc);
    std::cout << "gt       state:" << std::endl << sc.x.transpose() << std::endl;
 
    {
      auto lsc = lscs.back();
      try
      {
        std::cout << "lkf_new  state:" << std::endl << lsc.x.transpose() << std::endl;
        lsc = lkf.predict(lsc, u, Q, dt);
        std::cout << "lkf_new  predi:" << std::endl << lsc.x.transpose() << std::endl;
        lscs.push_back(lsc);
        lsc = lkf.correct(lsc, z, R);
        std::cout << "lkf_new  corre:" << std::endl << lsc.x.transpose() << std::endl;
        double sum2 = 0.0;
        for (const auto it : nconst_idxs)
          sum2 += lsc.x(it)*lsc.x(it);
        std::cout << "lkf_new  corre norm:" << std::sqrt(sum2) << std::endl;
        lscs.push_back(lsc);
      }
      catch (const std::exception& e)
      {
        ROS_ERROR("NEW  LKF failed: %s", e.what());
      }
    }
  }
 
  double x_diff = 0.0;
  double P_diff = 0.0;
  for (int it = 0; it < n_its; it++)
  {
    const auto lsc = lscs.at(it);
    const auto cur_x_diff = (lsc.x-lsc.x).norm();
    const auto cur_P_diff = (lsc.P-lsc.P).norm();
    x_diff += cur_x_diff;
    P_diff += cur_P_diff;
  }
 
  std::cout << "x diff average: " << x_diff/(n_its+1) << std::endl;
  std::cout << "P diff average: " << P_diff/(n_its+1) << std::endl;
 
  return 0;
}
 

Constructor & Destructor Documentation

◆ NCLKF_partial() [1/2]

template<int n_states, int n_inputs, int n_measurements, int n_norm_constrained_states>

mrs_lib::NCLKF_partial< n_states, n_inputs, n_measurements, n_norm_constrained_states >::NCLKF_partial ( )

inline

Convenience default constructor.

This constructor should not be used if applicable. If used, the main constructor has to be called afterwards, before using this class, otherwise the object is invalid (not initialized).

◆ NCLKF_partial() [2/2]

template<int n_states, int n_inputs, int n_measurements, int n_norm_constrained_states>

mrs_lib::NCLKF_partial< n_states, n_inputs, n_measurements, n_norm_constrained_states >::NCLKF_partial	(	const A_t &	A,
		const B_t &	B,
		const H_t &	H,
		const double	l,
		const indices_t &	norm_constrained_indices
	)

inline

The main constructor.

Parameters

A	State transition matrix of the system (n x n).
B	Input to state mapping matrix of the system (n x m).
H	State to measurement mapping matrix of the system (p x n).
l	The norm constraint, applied to the specified states.
norm_constrained_indices	Indices of the norm-constrained states in the state vector.

The documentation for this class was generated from the following file:

include/mrs_lib/nckf.h

Public Types

Public Member Functions

Static Public Attributes

Protected Member Functions

Additional Inherited Members

Detailed Description

template<int n_states, int n_inputs, int n_measurements, int n_norm_constrained_states> class mrs_lib::NCLKF_partial< n_states, n_inputs, n_measurements, n_norm_constrained_states >

Constructor & Destructor Documentation

◆ NCLKF_partial() [1/2]

◆ NCLKF_partial() [2/2]

template<int n_states, int n_inputs, int n_measurements, int n_norm_constrained_states>
class mrs_lib::NCLKF_partial< n_states, n_inputs, n_measurements, n_norm_constrained_states >