lar_content::KalmanFilter2D Class Reference

KalmanFilter2D class. More...

#include "KalmanFilter.h"

Public Member Functions
	KalmanFilter2D (double dt, double processVariance, double measurementVariance, const Eigen::VectorXd &x)
	Default constructor. More...
	~KalmanFilter2D ()
	Destructor. More...
void	Predict ()
	Uses the state transition matrix to predict the next state. More...
void	Update (const Eigen::VectorXd &z)
	Uses the measurement matrix to update the state estimate. More...
const Eigen::VectorXd &	GetState () const
	Get the current state vector. More...
const Eigen::VectorXd	GetPosition () const
	Get the current position vector. More...
const Eigen::VectorXd	GetDirection () const
	Get the current direction vector. More...
const Eigen::VectorXd &	GetTemporaryState () const
	Get the temporary state vector. More...

Private Attributes
double	m_dt
	Time step. More...
Eigen::VectorXd	m_x
	State vector (tracks position and 'velocity') More...
Eigen::MatrixXd	m_P
	Covariance matrix. More...
Eigen::MatrixXd	m_F
	State transition matrix. More...
Eigen::MatrixXd	m_H
	Measurement matrix. More...
Eigen::MatrixXd	m_R
	Measurement covariance matrix. More...
Eigen::MatrixXd	m_Q
	Process covariance matrix. More...
Eigen::VectorXd	m_xTemp
	Temporary state vector. More...
Eigen::MatrixXd	m_PTemp
	Temporary covariance matrix. More...

Detailed Description

KalmanFilter2D class.

Definition at line 92 of file KalmanFilter.h.

Constructor & Destructor Documentation

lar_content::KalmanFilter2D::KalmanFilter2D	(	double	dt,
		double	processVariance,
		double	measurementVariance,
		const Eigen::VectorXd &	x
	)

Default constructor.

Parameters

dt	Time step
processVariance	Process variance
measurementVariance	Measurement variance
x	Initial position

Definition at line 98 of file KalmanFilter.cc.

References m_F, m_H, m_P, m_Q, m_R, m_x, and x.

                                                                                                               :
    m_dt{dt}
{
    m_x = VectorXd::Zero(4);
    m_x.head(2) = x;
    m_P = MatrixXd::Identity(4, 4);
    m_F = MatrixXd::Identity(4, 4);
    for (int i = 0; i < 2; ++i)
    {
        m_F(i, i + 2) = dt;
    }
    m_H = MatrixXd::Zero(2, 4);
    m_H.block<2, 2>(0, 0) = MatrixXd::Identity(3, 3);
    m_Q = MatrixXd::Zero(4, 4);
    for (int i = 0; i < 2; ++i)
    {
        m_Q(i, i) = (0.25 * dt * dt * dt * dt) * processVariance;
        m_Q(i, i + 2) = (0.5 * dt * dt * dt) * processVariance;
        m_Q(i + 2, i) = (0.5 * dt * dt * dt) * processVariance;
        m_Q(i + 2, i + 2) = (dt * dt) * processVariance;
    }
    m_R = MatrixXd::Identity(2, 2) * measurementVariance;
}

lar_content::KalmanFilter2D::~KalmanFilter2D

(

)

Destructor.

Definition at line 124 of file KalmanFilter.cc.

{
}

Member Function Documentation

const VectorXd lar_content::KalmanFilter2D::GetDirection

(

)

const

Get the current direction vector.

Returns

The current direction vector

Definition at line 165 of file KalmanFilter.cc.

References m_x.

{
    return m_x.tail(2).normalized();
}

const VectorXd lar_content::KalmanFilter2D::GetPosition

(

)

const

Get the current position vector.

Returns

The current postion vector

Definition at line 158 of file KalmanFilter.cc.

References m_x.

{
    return m_x.head(2);
}

const VectorXd & lar_content::KalmanFilter2D::GetState

(

)

const

Get the current state vector.

Returns

The current state vector

Definition at line 151 of file KalmanFilter.cc.

References m_x.

{
    return m_x;
}

const VectorXd & lar_content::KalmanFilter2D::GetTemporaryState

(

)

const

Get the temporary state vector.

Returns

The temporary state vector

Definition at line 172 of file KalmanFilter.cc.

References m_xTemp.

{
    return m_xTemp;
}

void lar_content::KalmanFilter2D::Predict

(

)

Uses the state transition matrix to predict the next state.

Definition at line 130 of file KalmanFilter.cc.

References m_F, m_P, m_PTemp, m_Q, m_x, and m_xTemp.

{
    m_xTemp = m_F * m_x;
    m_PTemp = m_F * m_P * m_F.transpose() + m_Q;
}

void lar_content::KalmanFilter2D::Update

(

const Eigen::VectorXd &

z

)

Uses the measurement matrix to update the state estimate.

Parameters

z	The position to use for updating the state estimate

Definition at line 138 of file KalmanFilter.cc.

References m_H, m_P, m_PTemp, m_R, m_x, m_xTemp, and y.

{
    m_x = m_xTemp;
    m_P = m_PTemp;
    VectorXd y{z - m_H * m_x};
    MatrixXd S{m_H * m_P * m_H.transpose() + m_R};
    MatrixXd K{m_P * m_H.transpose() * S.inverse()};
    m_x += K * y;
    m_P = (MatrixXd::Identity(4, 4) - K * m_H) * m_P;
}

Member Data Documentation

double lar_content::KalmanFilter2D::m_dt

private

Time step.

Definition at line 151 of file KalmanFilter.h.

Eigen::MatrixXd lar_content::KalmanFilter2D::m_F

private

State transition matrix.

Definition at line 154 of file KalmanFilter.h.

Referenced by KalmanFilter2D(), and Predict().

Eigen::MatrixXd lar_content::KalmanFilter2D::m_H

private

Measurement matrix.

Definition at line 155 of file KalmanFilter.h.

Referenced by KalmanFilter2D(), and Update().

Eigen::MatrixXd lar_content::KalmanFilter2D::m_P

private

Covariance matrix.

Definition at line 153 of file KalmanFilter.h.

Referenced by KalmanFilter2D(), Predict(), and Update().

Eigen::MatrixXd lar_content::KalmanFilter2D::m_PTemp

private

Temporary covariance matrix.

Definition at line 159 of file KalmanFilter.h.

Referenced by Predict(), and Update().

Eigen::MatrixXd lar_content::KalmanFilter2D::m_Q

private

Process covariance matrix.

Definition at line 157 of file KalmanFilter.h.

Referenced by KalmanFilter2D(), and Predict().

Eigen::MatrixXd lar_content::KalmanFilter2D::m_R

private

Measurement covariance matrix.

Definition at line 156 of file KalmanFilter.h.

Referenced by KalmanFilter2D(), and Update().

Eigen::VectorXd lar_content::KalmanFilter2D::m_x

private

State vector (tracks position and 'velocity')

Definition at line 152 of file KalmanFilter.h.

Referenced by GetDirection(), GetPosition(), GetState(), KalmanFilter2D(), Predict(), and Update().

Eigen::VectorXd lar_content::KalmanFilter2D::m_xTemp

private

Temporary state vector.

Definition at line 158 of file KalmanFilter.h.

Referenced by GetTemporaryState(), Predict(), and Update().

---

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

• larpandoracontent/v04_15_01/source/larpandoracontent/LArUtility/KalmanFilter.h
• larpandoracontent/v04_15_01/source/larpandoracontent/LArUtility/KalmanFilter.cc