C++ for Scientists - Technische Universität Dresden

14.7. SEQUENTIAL NOISE REDUCTION OF REAL-TIME MEASUREMENTS BY LEAST SQUARES283
we have, with gk = A T k bk, that
A T k+1 Ak+1xk+1 = gk+1
(A T k Ak + ak+1a T k+1 )xk+1 = gk + ak+1f(tk+1)
With Mk = (A T k Ak) −1 ∈ R n×n , we derive from the Sherman-Morrison formula, that
and we also have that
Mk+1 := (A T k Ak + ak+1a T k+1 )−1 = Mk − Mkak+1a T k+1 Mk
1 + a T k+1 Mkak+1
xk+1 = Mk+1(gk + ak+1f(tk+1)) = Mkgk + Mkak+1f(tk+1) − Mkak+1aT k+1
1 + aT k+1Mkak+1 (Mkgk + Mkak+1f(tk+1))
= xk +
Mkak+1
1 + aT k+1Mkak+1 (f(tk+1) − a T k+1xk) The Kalman method works as follows. We can use the LAPACK subroutine DGESV for com-
1
2
3
4
5
6
7
1. Solve Anxn = bn by taking the first n rows of A and b
2. Let Mn = A−1 n A−T n
3. For k = n + 1, . . . , m do:
3.1. Compute the Kalman gain vector kk+1 = Mkak+1/(1 + aT k+1Mkak+1). 3.2. Update step: xk+1 = xk + kk+1(f(tk+1) − aT k+1xk). 3.3. Mk+1 = Mk − kkaT k+1Mk. puting A −1
n .
14.7.4 Software
The goal is to write a function that computes the Kalman filter least squares. Because of the
sequential character, we suggest to make a class with the following specifications:
template
class kalman {
public:
// Creation of the Kalman filter
kalman( int n ) ;
// Compute the first n observations and initialize the Kalman
// filter (Steps 1 and 2 in the algorithm)
// BaseFun is a binary functor.
template
void initialize( VIt t begin, VIt const& t end, BaseFun& base fun, F& f ) {
...
}
template