1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
|
/*=========================================================================
Program: Insight Segmentation & Registration Toolkit
Module: $RCSfile: itkKalmanLinearEstimator.h,v $
Language: C++
Date: $Date: 2005-05-06 18:53:39 $
Version: $Revision: 1.16 $
Copyright (c) Insight Software Consortium. All rights reserved.
See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.
This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. See the above copyright notices for more information.
=========================================================================*/
#ifndef __itkKalmanLinearEstimator_h
#define __itkKalmanLinearEstimator_h
#include "itkMacro.h"
#include <vnl/vnl_vector_fixed.h>
#include <vnl/vnl_matrix_fixed.h>
namespace itk
{
/** \class KalmanLinearEstimator
* \brief Implement a linear recursive estimator.
*
* KalmanLinearEstimator class implements a linear recursive estimator. The
* class is templated over the type of the parameters to be estimated and
* over the number of parameters. Recursive estimation is a fast mechanism
* for getting information about a system for which we only have access to
* measures that are linearly related with the parameters we want to
* estimate.
*
* \ingroup Numerics
*/
template <class T, unsigned int VEstimatorDimension>
class ITK_EXPORT KalmanLinearEstimator
{
public:
/** Dimension of the vector of parameters to be estimated.
* It is equivalent to the number of parameters to estimate. */
itkStaticConstMacro( Dimension, unsigned int,
VEstimatorDimension);
/** Vector type defines a generic vector type that is used
* for the matricial operations performed during estimation. */
typedef vnl_vector_fixed<T,VEstimatorDimension> VectorType;
/** Matrix type defines a generic matrix type that is used
* for the matricial operations performed during estimation. */
typedef vnl_matrix_fixed<T,VEstimatorDimension,VEstimatorDimension> MatrixType;
/** Type is the type associated with the parameters to be estimated.
* All the parameters are of the same type. Natural choices could be
* floats and doubles, because Type also is used for all the internal
* computations. */
typedef T ValueType;
/** Update the estimation using the information provided by a new measure
* along with a new line of the linear predictor. This method is the one
* that should be called iteratively in order to estimate the parameter's
* vector. It internally updates the covariance matrix. */
void UpdateWithNewMeasure( const ValueType & newMeasure,
const VectorType & newPredictor );
/** This method resets the estimator. It set all the parameters to null.
* The covariance matrix is not changed.
* \sa Estimator \sa Variance \sa ClearVariance */
void ClearEstimation(void)
{ m_Estimator = VectorType(T(0)); }
/** This method resets the covariance matrix. It is set to an identity matrix
* \sa Estimator \sa Variance \sa ClearEstimation */
void ClearVariance(void)
{
m_Variance.set_identity();
}
/** This method sets the covariance matrix to a diagonal matrix with
* equal values. It is useful when the variance of all the parameters
* be estimated are the same and the parameters are considered independents.
* \sa Estimator
* \sa Variance
* \sa ClearEstimation */
void SetVariance(const ValueType & var = 1.0)
{
m_Variance.set_identity();
m_Variance *= var;
}
/** This method sets the covariance matrix to known matrix. It is intended to
* initialize the estimator with a priori information about the statistical
* distribution of the parameters. It can also be used to resume the
* operation of a previously used estimator using it last known state.
* \sa Estimator \sa Variance \sa ClearEstimation */
void SetVariance(const MatrixType & m)
{ m_Variance = m; }
/** This method returns the vector of estimated parameters
* \sa Estimator */
const VectorType & GetEstimator(void) const
{ return m_Estimator; }
/** This method returns the covariance matrix of the estimated parameters
* \sa Variance */
const MatrixType & GetVariance(void) const
{ return m_Variance; }
private:
/** This methods performs the update of the parameter's covariance matrix.
* It is called by updateWithNewMeasure() method. Users are not expected to
* call this method directly.
* \sa updateWithNewMeasure */
void UpdateVariance( const VectorType & );
/** Vector of parameters to estimate.
* \sa GetEstimator */
VectorType m_Estimator;
/** Estimation of the parameter's covariance matrix. This matrix contains
* the information about the estate of the estimator. It holds all the
* information obtained from previous measures provided to the
* estimator. The initialization of this matrix is critical to the behavior
* of the estimator, at least to ensure a short trasient period for
* estabilizing the estimation. \sa SetVariance \sa GetVariance */
MatrixType m_Variance;
};
template <class T, unsigned int VEstimatorDimension>
void
KalmanLinearEstimator<T,VEstimatorDimension>
::UpdateWithNewMeasure( const ValueType & newMeasure,
const VectorType & newPredictor )
{
ValueType measurePrediction = dot_product( newPredictor , m_Estimator );
ValueType errorMeasurePrediction = newMeasure - measurePrediction;
VectorType Corrector = m_Variance * newPredictor;
for( unsigned int j=0; j<VEstimatorDimension; j++)
{
m_Estimator(j) += Corrector(j) * errorMeasurePrediction;
}
UpdateVariance( newPredictor );
}
template <class T, unsigned int VEstimatorDimension>
void
KalmanLinearEstimator<T,VEstimatorDimension>
::UpdateVariance( const VectorType & newPredictor )
{
VectorType aux = m_Variance * newPredictor;
ValueType denominator = 1.0/(1.0 + dot_product( aux , newPredictor ) );
for( unsigned int col=0; col<VEstimatorDimension; col++)
{
for( unsigned int row=0; row<VEstimatorDimension; row++)
{
m_Variance(col,row) -= aux(col)*aux(row)*denominator;
}
}
}
} // end namespace itk
#endif
|
