lar_content::KalmanFilter3D Class Reference

KalmanFilter3D class. More...

#include "KalmanFilter.h"

Public Member Functions
	KalmanFilter3D (double dt, double processVariance, double measurementVariance, const Eigen::VectorXd &x)
	Default constructor. More...
	~KalmanFilter3D ()
	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

KalmanFilter3D class.

Definition at line 19 of file KalmanFilter.h.

Constructor & Destructor Documentation

lar_content::KalmanFilter3D::KalmanFilter3D	(	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 16 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(6);
    m_x.head(3) = x;
    m_P = MatrixXd::Identity(6, 6);
    m_F = MatrixXd::Identity(6, 6);
    for (int i = 0; i < 3; ++i)
    {
        m_F(i, i + 3) = dt;
    }
    m_H = MatrixXd::Zero(3, 6);
    m_H.block<3, 3>(0, 0) = MatrixXd::Identity(3, 3);
    m_Q = MatrixXd::Zero(6, 6);
    for (int i = 0; i < 3; ++i)
    {
        m_Q(i, i) = (0.25 * dt * dt * dt * dt) * processVariance;
        m_Q(i, i + 3) = (0.5 * dt * dt * dt) * processVariance;
        m_Q(i + 3, i) = (0.5 * dt * dt * dt) * processVariance;
        m_Q(i + 3, i + 3) = (dt * dt) * processVariance;
    }
    m_R = MatrixXd::Identity(3, 3) * measurementVariance;
}

lar_content::KalmanFilter3D::~KalmanFilter3D

(

)

Destructor.

Definition at line 42 of file KalmanFilter.cc.

{
}

Member Function Documentation

const VectorXd lar_content::KalmanFilter3D::GetDirection

(

)

const

Get the current direction vector.

Returns

The current direction vector

Definition at line 83 of file KalmanFilter.cc.

References m_x.

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

const VectorXd lar_content::KalmanFilter3D::GetPosition

(

)

const

Get the current position vector.

Returns

The current postion vector

Definition at line 76 of file KalmanFilter.cc.

References m_x.

{
    return m_x.head(3);
}

const VectorXd & lar_content::KalmanFilter3D::GetState

(

)

const

Get the current state vector.

Returns

The current state vector

Definition at line 69 of file KalmanFilter.cc.

References m_x.

{
    return m_x;
}

const VectorXd & lar_content::KalmanFilter3D::GetTemporaryState

(

)

const

Get the temporary state vector.

Returns

The temporary state vector

Definition at line 90 of file KalmanFilter.cc.

References m_xTemp.

{
    return m_xTemp;
}

void lar_content::KalmanFilter3D::Predict

(

)

Uses the state transition matrix to predict the next state.

Definition at line 48 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::KalmanFilter3D::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 56 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(6, 6) - K * m_H) * m_P;
}

Member Data Documentation

double lar_content::KalmanFilter3D::m_dt

private

Time step.

Definition at line 78 of file KalmanFilter.h.

Eigen::MatrixXd lar_content::KalmanFilter3D::m_F

private

State transition matrix.

Definition at line 81 of file KalmanFilter.h.

Referenced by KalmanFilter3D(), and Predict().

Eigen::MatrixXd lar_content::KalmanFilter3D::m_H

private

Measurement matrix.

Definition at line 82 of file KalmanFilter.h.

Referenced by KalmanFilter3D(), and Update().

Eigen::MatrixXd lar_content::KalmanFilter3D::m_P

private

Covariance matrix.

Definition at line 80 of file KalmanFilter.h.

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

Eigen::MatrixXd lar_content::KalmanFilter3D::m_PTemp

private

Temporary covariance matrix.

Definition at line 86 of file KalmanFilter.h.

Referenced by Predict(), and Update().

Eigen::MatrixXd lar_content::KalmanFilter3D::m_Q

private

Process covariance matrix.

Definition at line 84 of file KalmanFilter.h.

Referenced by KalmanFilter3D(), and Predict().

Eigen::MatrixXd lar_content::KalmanFilter3D::m_R

private

Measurement covariance matrix.

Definition at line 83 of file KalmanFilter.h.

Referenced by KalmanFilter3D(), and Update().

Eigen::VectorXd lar_content::KalmanFilter3D::m_x

private

State vector (tracks position and 'velocity')

Definition at line 79 of file KalmanFilter.h.

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

Eigen::VectorXd lar_content::KalmanFilter3D::m_xTemp

private

Temporary state vector.

Definition at line 85 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