Skip to content

Commit e444c45

Browse files
cassinajcassinaj
committed
Added FACTORIZED_STATES parameter
Fixed Prediction Extended few docs
1 parent c2554d5 commit e444c45

File tree

1 file changed

+102
-42
lines changed

1 file changed

+102
-42
lines changed

‎include/fast_filtering/filters/deterministic/factorized_unscented_kalman_filter.hpp

+102-42
Original file line numberDiff line numberDiff line change
@@ -72,13 +72,14 @@ namespace ff
7272
*/
7373
template<typename CohesiveStateProcessModel,
7474
typename FactorizedStateProcessModel,
75-
typename ObservationModel>
75+
typename ObservationModel,
76+
size_t FACTORIZED_STATES = -1>
7677
class FactorizedUnscentedKalmanFilter
7778
{
7879
public:
7980
typedef ComposedStateDistribution<typename CohesiveStateProcessModel::State,
8081
typename FactorizedStateProcessModel::State,
81-
1> StateDistribution;
82+
FACTORIZED_STATES> StateDistribution;
8283

8384
typedef typename StateDistribution::CovAA CovAA;
8485
typedef typename StateDistribution::CovBB CovBB;
@@ -91,6 +92,9 @@ class FactorizedUnscentedKalmanFilter
9192
Eigen::Dynamic,
9293
Eigen::Dynamic> SigmaPoints;
9394

95+
typedef typename CohesiveStateProcessModel::State State_a;
96+
typedef typename FactorizedStateProcessModel::State State_b_i;
97+
9498
typedef boost::shared_ptr<CohesiveStateProcessModel> CohesiveStateProcessModelPtr;
9599
typedef boost::shared_ptr<FactorizedStateProcessModel> FactorizedStateProcessModelPtr;
96100
typedef boost::shared_ptr<ObservationModel> ObservationModelPtr;
@@ -108,34 +112,36 @@ class FactorizedUnscentedKalmanFilter
108112
h_(observation_model),
109113
kappa_(kappa)
110114
{
111-
112115
alpha_ = 1.2;
113116
beta_ = 2.;
114117
kappa_ = 0.;
115118
}
116119

117120
virtual ~FactorizedUnscentedKalmanFilter() { }
118121

122+
/**
123+
* Predicts the state for the next time step
124+
*
125+
* @param [in] prior_state State prior distribution
126+
* @param [out] predicted_state Predicted state posterior distribution
127+
*/
119128
void predict(const StateDistribution& prior_state,
120129
StateDistribution& predicted_state)
121130
{
122131
// compute the sigma point partitions X = [Xa XQa 0(b^[i]) XQb XR]
123132
// 0(b^[i]) is the place holder for the a b^[i]
124133
ComputeSigmaPointPartitions(
125134
{
126-
{prior_state.a_, prior_state.cov_aa_},
127-
{Eigen::MatrixXd::Zero(Dim(Q_a), 1), f_a_->NoiseCovariance()},
128-
{Eigen::MatrixXd::Zero(Dim(b_i), 1), Eigen::MatrixXd::Zero(Dim(b_i), Dim(b_i))},
129-
{Eigen::MatrixXd::Zero(Dim(Q_bi), 1), f_b_->NoiseCovariance()},
130-
{Eigen::MatrixXd::Zero(Dim(R_yi), 1), h_->NoiseCovariance()(0, 0)}
135+
{ prior_state.a_, prior_state.cov_aa_ },
136+
{ Eigen::MatrixXd::Zero(Dim(Q_a), 1), f_a_->NoiseCovariance() },
137+
{ Eigen::MatrixXd::Zero(Dim(b_i), 1), Eigen::MatrixXd::Zero(Dim(b_i), Dim(b_i)) },
138+
{ Eigen::MatrixXd::Zero(Dim(Q_bi), 1), f_b_->NoiseCovariance() },
139+
{ Eigen::MatrixXd::Zero(Dim(R_yi), 1), h_->NoiseCovariance() }
131140
},
132141
X_);
133142

134-
// X_[Q_a];
135-
// X_[Q_bi];
136-
// X_[R_yi];
137-
138-
/* X_[a] = f_a->predict(X_[a], X_[Q_bi]) */
143+
// FOR ALL X_[a]
144+
// X_[a] = f_a_ -> predict(X_[a], X_[Q_bi]);
139145

140146
// predict the cohesive state segment a
141147
Mean(X_[a], predicted_state.a_);
@@ -150,8 +156,9 @@ class FactorizedUnscentedKalmanFilter
150156
Dim(a) + Dim(Q_a),
151157
X_[b_i]);
152158

153-
/* X_[b_i] = f_a->predict(X_[b_i], X_[Q_bi]) */
154-
/* X_[y_i] = h->predict(X_[a], X_[Q_bi], X_[R_yi]) */
159+
// FOR ALL X_[b_i] and X_[y_i]
160+
//X_[b_i] = f_b_ -> predict(X_[b_i], X_[Q_bi]);
161+
//X_[y_i] = h_ -> predict(X_[a], X_[Q_bi], X_[R_yi]);
155162

156163
typename StateDistribution::JointPartitions& predicted_partition =
157164
predicted_state.joint_partitions_[i];
@@ -314,26 +321,14 @@ class FactorizedUnscentedKalmanFilter
314321
case b_i: return f_b_->Dimension();
315322
case Q_bi: return f_b_->NoiseDimension();
316323
case y_i: return 1;
317-
case R_yi: return 1;
324+
//case R_yi: return 1;
318325
}
319326
}
320327

321-
/**
322-
* Returns the number of sigma points accroding to the specified random
323-
* variable list. E.g. {a, b_i, y_i} results in
324-
* 2*(dim(a)+dim(b_i)+dim(y_i)) + 1
325-
*
326-
* @param var_ids The list of random variable IDs
327-
*/
328-
size_t CountSigmas(const std::vector<RandomVariableIndex>& var_ids)
329-
{
330-
size_t sigmas = 0;
331-
for (auto& var_id: var_ids) { sigmas += Dim(var_id); }
332-
return 2 * sigmas + 1;
333-
}
334-
335328
/**
336329
* Computes the weighted mean of the given sigma points
330+
*
331+
* @note TESTED
337332
*/
338333
template <typename MeanVector>
339334
void Mean(const SigmaPoints& sigma_points, MeanVector& mean)
@@ -357,6 +352,8 @@ class FactorizedUnscentedKalmanFilter
357352
* @param [in] w Weights of the points used to determine the
358353
* covariance
359354
* @param [out] sigma_points The sigma point collection
355+
*
356+
* @note TESTED
360357
*/
361358
template <typename MeanVector>
362359
void Normalize(const MeanVector& mean, SigmaPoints& sigma_points)
@@ -385,6 +382,8 @@ class FactorizedUnscentedKalmanFilter
385382
* @param [in] number_of_sigma_points
386383
* @param [out] w_0_sqrt The weight for the first sigma point
387384
* @param [out] w_i_sqrt The weight for the remaining sigma points
385+
*
386+
* @note TESTED
388387
*/
389388
void ComputeWeights(size_t number_of_sigma_points,
390389
double& w_0,
@@ -406,6 +405,8 @@ class FactorizedUnscentedKalmanFilter
406405
* (the mean) must have the required number
407406
* of rows and 0 columns.
408407
* @param sigma_point_partitions sigma point partitions
408+
*
409+
* @note TESTED
409410
*/
410411
void ComputeSigmaPointPartitions(
411412
const std::vector<std::pair<Eigen::MatrixXd,
@@ -447,6 +448,8 @@ class FactorizedUnscentedKalmanFilter
447448
* @param [in] offset Offset dimension if this transform is a
448449
* partition of a larger one
449450
* @param [out] sigma_points Selected sigma points
451+
*
452+
* @note TESTED
450453
*/
451454
template <typename MeanVector, typename CovarianceMatrix>
452455
void ComputeSigmaPoints(const MeanVector& mean,
@@ -483,13 +486,19 @@ class FactorizedUnscentedKalmanFilter
483486
}
484487
}
485488

489+
/**
490+
* @note TESTED
491+
*/
486492
template <typename RegularMatrix, typename SquareRootMatrix>
487493
void SquareRoot(const RegularMatrix& regular_matrix,
488494
SquareRootMatrix& square_root)
489495
{
490496
square_root = regular_matrix.llt().matrixL();
491497
}
492498

499+
/**
500+
* @note TESTED
501+
*/
493502
template <typename RegularMatrix>
494503
void SquareRootDiagonal(const RegularMatrix& regular_matrix,
495504
RegularMatrix& square_root)
@@ -501,6 +510,9 @@ class FactorizedUnscentedKalmanFilter
501510
}
502511
}
503512

513+
/**
514+
* @note TESTED
515+
*/
504516
template <typename RegularMatrix, typename SquareRootVector>
505517
void SquareRootDiagonalAsVector(const RegularMatrix& regular_matrix,
506518
SquareRootVector& square_root)
@@ -526,9 +538,33 @@ class FactorizedUnscentedKalmanFilter
526538

527539
/**
528540
* Blockweise matrix inversion using the Sherman-Morrision-Woodbury
529-
* indentity given that A^-1 is already available
530-
* | A B |-1 | L_A L_B |
531-
* | C D | = | L_C L_D |
541+
* indentity given that \f$\Sigma^{-1}_{aa}\f$ of
542+
*
543+
* \f$
544+
* \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
545+
* \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
546+
* \f$
547+
*
548+
* is already available.
549+
*
550+
* \f$
551+
* \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
552+
* \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
553+
* \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
554+
* \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
555+
* \f$
556+
*
557+
* @param [in] A_inv \f$ \Sigma^{-1}_{aa} \f$
558+
* @param [in] B \f$ \Sigma_{ab} \f$
559+
* @param [in] C \f$ \Sigma_{ba} \f$
560+
* @param [in] D \f$ \Sigma_{bb} \f$
561+
* @param [out] L_A \f$ \Lambda_{aa} \f$
562+
* @param [out] L_B \f$ \Lambda_{ab} \f$
563+
* @param [out] L_C \f$ \Lambda_{ba} \f$
564+
* @param [out] L_D \f$ \Lambda_{bb} \f$
565+
* @param [out] L \f$ \Lambda \f$
566+
*
567+
* @note TESTED
532568
*/
533569
template <typename MatrixAInv,
534570
typename MatrixB,
@@ -561,9 +597,33 @@ class FactorizedUnscentedKalmanFilter
561597

562598
/**
563599
* Blockweise matrix inversion using the Sherman-Morrision-Woodbury
564-
* indentity given that A^-1 is already available
565-
* | A B |-1 | L_A L_B |
566-
* | C D | = | L_C L_D | = inv
600+
* indentity given that \f$\Sigma^{-1}_{aa}\f$ of
601+
*
602+
* \f$
603+
* \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
604+
* \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
605+
* \f$
606+
*
607+
* is already available.
608+
*
609+
* \f$
610+
* \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
611+
* \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
612+
* \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
613+
* \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
614+
* \f$
615+
*
616+
* @param [in] A_inv \f$ \Sigma^{-1}_{aa} \f$
617+
* @param [in] B \f$ \Sigma_{ab} \f$
618+
* @param [in] C \f$ \Sigma_{ba} \f$
619+
* @param [in] D \f$ \Sigma_{bb} \f$
620+
* @param [out] L_A \f$ \Lambda_{aa} \f$
621+
* @param [out] L_B \f$ \Lambda_{ab} \f$
622+
* @param [out] L_C \f$ \Lambda_{ba} \f$
623+
* @param [out] L_D \f$ \Lambda_{bb} \f$
624+
* @param [out] L \f$ \Lambda \f$
625+
*
626+
* @note TESTED
567627
*/
568628
template <typename MatrixAInv,
569629
typename MatrixB,
@@ -582,16 +642,16 @@ class FactorizedUnscentedKalmanFilter
582642
MatrixLB& L_B,
583643
MatrixLC& L_C,
584644
MatrixLD& L_D,
585-
ResultMatrix& inv)
645+
ResultMatrix& L)
586646
{
587647
SMWInversion(A_inv, B, C, D, L_A, L_B, L_C, L_D);
588648

589-
inv.resize(L_A.rows() + L_C.rows(), L_A.cols() + L_B.cols());
649+
L.resize(L_A.rows() + L_C.rows(), L_A.cols() + L_B.cols());
590650

591-
inv.block(0, 0, L_A.rows(), L_A.cols()) = L_A;
592-
inv.block(0, L_A.cols(), L_B.rows(), L_B.cols()) = L_B;
593-
inv.block(L_A.rows(), 0, L_C.rows(), L_C.cols()) = L_C;
594-
inv.block(L_A.rows(), L_A.cols(), L_D.rows(), L_D.cols()) = L_D;
651+
L.block(0, 0, L_A.rows(), L_A.cols()) = L_A;
652+
L.block(0, L_A.cols(), L_B.rows(), L_B.cols()) = L_B;
653+
L.block(L_A.rows(), 0, L_C.rows(), L_C.cols()) = L_C;
654+
L.block(L_A.rows(), L_A.cols(), L_D.rows(), L_D.cols()) = L_D;
595655
}
596656

597657
protected:

0 commit comments

Comments
 (0)