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
|
/*
* =====================================================================================
*
* Filename: state.cpp
*
* Description: A Class for managing body and features.
*
* Version: 1.0
* Created: 03/17/2017 07:55:56 PM
* Revision: none
* Compiler: gcc
*
* Author: Martin Miller (MHM), miller7@illinois.edu
* Organization: Aerospace Robotics and Controls Lab (ARC)
*
* =====================================================================================
*/
#include "state.h"
void
State::update (Matrix<double,9,1> &X, Matrix<double,9,9> &P,
const Matrix<double,1,9> H, const Matrix<double,1,1> &h,
const Matrix<double,1,1> &z, const Matrix<double,1,1> &R)
{
Matrix<double,1,1> S;
Matrix<double,9,1> K;
S = H*P*H.transpose() + R;
K = P*H.transpose()*S.inverse();
P = (Matrix<double,9,9>::Identity()-K*H)*P;
P = 0.5*(P+P.transpose());
X += K*(z-h);
}
/*
*--------------------------------------------------------------------------------------
* Class: State
* Method: State :: Pkk1
* Description: Updates P_k|k-1
*--------------------------------------------------------------------------------------
*/
void
State::Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F,
const Matrix<double,9,9> &Q )
{
P = F*P*F.transpose()+Q;
// Enforce symmetry
P = 0.5*(P+P.transpose());
return ;
} /* ----- end of method State::Pkk1 ----- */
|
