110 lines
3.2 KiB
C++
110 lines
3.2 KiB
C++
/* @file KalmanFilterObservable.hpp */
|
|
|
|
#pragma once
|
|
|
|
#include "time/Time.hpp"
|
|
#include <Eigen/Dense>
|
|
#include <cstdint>
|
|
|
|
namespace xo {
|
|
namespace kalman {
|
|
class KalmanFilterObservable {
|
|
public:
|
|
using MatrixXd = Eigen::MatrixXd;
|
|
using VectorXi = Eigen::VectorXi;
|
|
|
|
public:
|
|
KalmanFilterObservable() = default;
|
|
|
|
/* keep maps back to canonical observations z(j).
|
|
* H, R have been re-indexed
|
|
*
|
|
* If all m observations are included, then keep will be:
|
|
* [0, .., m-1]
|
|
*/
|
|
KalmanFilterObservable(VectorXi keep, MatrixXd H, MatrixXd R)
|
|
: keep_{std::move(keep)}, H_{std::move(H)}, R_{std::move(R)} {
|
|
assert(this->check_ok());
|
|
} /*ctor*/
|
|
|
|
/* build KF observable object where keeping all observations */
|
|
static KalmanFilterObservable keep_all(MatrixXd H, MatrixXd R);
|
|
|
|
/* build KF observable object. replace H, R with reindexed versions H', R'
|
|
* according to indexing vector keep[]. keep[] indexes members of
|
|
* observation vector z(k)[j]. Reindexed z', H', R' as follows:
|
|
*
|
|
* z'[j*] = z[keep[j*]]
|
|
* H'[j*, i] = H[keep[j*], i]
|
|
* R'[j1*, j2*] = R[keep[j1*], keep[j2*]]
|
|
*/
|
|
static KalmanFilterObservable reindex(VectorXi keep, MatrixXd H, MatrixXd R);
|
|
|
|
uint32_t n_state() const { return H_.cols(); }
|
|
uint32_t n_observable() const { return H_.rows(); }
|
|
VectorXi const & keep() const { return keep_; }
|
|
MatrixXd const & observable() const { return H_; }
|
|
MatrixXd const & observable_cov() const { return R_; }
|
|
|
|
bool check_ok() const {
|
|
uint32_t m = H_.rows();
|
|
bool keep_is_mx1 = (keep_.rows() == m);
|
|
bool r_is_mxm = ((R_.cols() == m) && (R_.rows() == m));
|
|
|
|
bool keep_is_well_ordered = true;
|
|
|
|
/* members of .keep are non-negative integers,
|
|
* in strictly increasing order
|
|
*/
|
|
int64_t keep_jm1 = -1;
|
|
|
|
for (uint32_t j = 0; j < keep_.rows(); ++j) {
|
|
if (keep_[j] < 0)
|
|
keep_is_well_ordered = false;
|
|
if (keep_[j] <= keep_jm1)
|
|
keep_is_well_ordered = false;
|
|
}
|
|
|
|
/* also would like to require: R is +ve definite */
|
|
|
|
return keep_is_mx1 && keep_is_well_ordered && r_is_mxm;
|
|
} /*check_ok*/
|
|
|
|
void display(std::ostream & os) const;
|
|
std::string display_string() const;
|
|
|
|
private:
|
|
|
|
private:
|
|
/* m: #of observations that survived sanity/error checks
|
|
*
|
|
* H, R here will have been re-indexed to exclude rejected observations.
|
|
* observations z will also have been re-indexed.
|
|
*
|
|
* If an observation z[j] is excluded, then also exclude:
|
|
* - j'th row of H
|
|
* - j'th row and j'th column of R
|
|
* - j'th element of z
|
|
*
|
|
* Given re-indexed H, R, the j*'th row of H goes with z[keep[j*]]
|
|
*/
|
|
|
|
/* [m x 1] maps back to indices in original observation vector */
|
|
VectorXi keep_;
|
|
/* [m x n] observation matrix */
|
|
MatrixXd H_;
|
|
/* [m x m] covariance matrix for observation noise */
|
|
MatrixXd R_;
|
|
}; /*KalmanFilterObservable*/
|
|
|
|
inline std::ostream &
|
|
operator<<(std::ostream & os, KalmanFilterObservable const & x)
|
|
{
|
|
x.display(os);
|
|
return os;
|
|
} /*operator<<*/
|
|
} /*namespace kalman*/
|
|
} /*namespace xo*/
|
|
|
|
/* end KalmanFilterObservable.hpp */
|
|
|