Roland Conybeare
9e93a96754
git-subtree-dir: xo-kalmanfilter git-subtree-mainline:a49aa0c1f1git-subtree-split:2ced8429c0
164 lines
5 KiB
Text
164 lines
5 KiB
Text
scaffold kalman filter implementation here
|
|
|
|
notation:
|
|
|
|
x_(k) : n x 1 : true system state that we want to estimate
|
|
F(k) : n x n : state transition matrix at time t(k)
|
|
w_(k) : n x 1 : system noise, with mean 0, covariance Q(k)
|
|
Q(k) : n x n : covariance of systen noise
|
|
|
|
v_(k) : m x 1 : observation errors at time t(k)
|
|
R(k) : m x m : observation error covariance matrix at time t(k)
|
|
|
|
x(k) : n x 1 : state vector estimate for time t(k)
|
|
P(k) : n x n : covariance matrix for x(k)
|
|
z(k) : m x 1 : observation vector at time t(k)
|
|
H(k) : m x n : observation matrix at time t(k)
|
|
|
|
K(k) : : kalman gain - measures information gain from observation z(k)
|
|
relative to prior P(k | k-1)
|
|
|
|
use shorthand:
|
|
xT, F(k)T for transpose(x), transpose(F(k))
|
|
x(k+1 | k) for "estimate x(k+1) given information known at t(k)"
|
|
|
|
A. Linear Kalman Filter
|
|
-----------------------
|
|
|
|
1. system model:
|
|
x_(k+1) = F(k).x_(k) + w_(k), w_(k) ~ N(0,Q)
|
|
|
|
i.e. expected behavior of system from t(k) -> t(k+1),
|
|
absent system noise, is given by linear transformation F(k)
|
|
|
|
ofc model is not directly observable,
|
|
since we don't know x_(k) or w_(k),
|
|
instead we will be estimating it.
|
|
|
|
2. measurement model:
|
|
z(k+1) = H(k+1).x_(k+1) + v_(k), v_(k) ~ N(0, R)
|
|
|
|
3. prior:
|
|
x(0), P(0)
|
|
|
|
must be supplied as initial input
|
|
|
|
4. pre-estimate for t(k+1) system state:
|
|
(before incorporating z(k+1) and accounting for system noise)
|
|
x(k+1|k) := F(k).x(k)
|
|
|
|
in other words propagate t(k) estimate to t(k+1),
|
|
using F(k)
|
|
|
|
5. pre-estimate for t(k+1) estimate covariance
|
|
(before incorporating z(k+1) and accounting for system noise)
|
|
P(k+1|k) := F(k).P(k).F(k)T + Q(k)
|
|
|
|
6. kalman gain matrix
|
|
K(k+1) := P(k+1|k).H(k)T.inverse(H(k).P(k+1|k).H(k)T + R(k))
|
|
|
|
note that the matrix-to-be-inverted in the gain expression
|
|
is symmetric and positive-definite (so can use cholesky decomposition)
|
|
|
|
7. innovation: difference between actual observation vector and
|
|
observation predicted from state estimate x(k+1|k):
|
|
z(k+1) - H(k+1).x(k+1|k)
|
|
|
|
8. corrected state estimate for t(k+1):
|
|
x(k+1) := x(k+1|k) + K(k+1)[z(k+1) - H(k+1).x(k+1|k)]
|
|
|
|
9. correct state covariance for t(k+1):
|
|
P(k+1) := [I - K(k+1).H(k+1)].P(k+1|k)
|
|
|
|
|
|
B. Extended Kalman Filter
|
|
-------------------------
|
|
Must write in continuous form, to deal with non-linearities
|
|
|
|
notation:
|
|
|
|
x_(t) : n x 1 : true system state that we want to estimate,
|
|
continuously evolves with t
|
|
f(x_(t),t) : n x 1 : expected derivative (d/dt)(x_(t))
|
|
h(x_(t),t) : m x 1 : observation function
|
|
x(k) : n x 1 : state vector estimate for x(t) at time t=t(k)
|
|
F(x(t),t) : n x n : jacobian of f(x_(t),t) evaluated at x_(t)=x(t)
|
|
H(x(t),t) : m x n : jacobian of h(x_(t),t) evaluated at x_(t)=x(t)
|
|
|
|
1. system model:
|
|
(d/dt)x_(t) = f(x_(t),t) + w(t),
|
|
where
|
|
w(t) is a white noise with
|
|
|
|
/ t2
|
|
|
|
|
| w(t).dt ~ N(0, Q(t1,t2))
|
|
|
|
|
/ t1
|
|
|
|
editorial:
|
|
note that we exclude more general model
|
|
(d/dt)x_(t) = f(x_(t),t)) + G(x_(t),t).w(t)
|
|
because would be unable to assume RHS mean-square integrable.
|
|
The form chosen effectively band-limits the frequencies at which
|
|
w(t) acts, since it's independent of x_(t). This allows mean-square
|
|
rules to be applied as in linear kalman filter
|
|
|
|
2. measurement model:
|
|
z(k+1) = h(x_(t(k+1)), t(k+1)) + v(k), v(k) ~ N(0, Rk)
|
|
|
|
3. prior:
|
|
x(0), P(0)
|
|
|
|
(same as for linear kalman filter)
|
|
|
|
(3a). state estimate propagation model:
|
|
(d/dt)x(t) = f(x(t),t)
|
|
|
|
editorial:
|
|
this step is *sketchy*: x(t) is intended to be minimum-variance
|
|
estimate for system state, but that property is lost when solving
|
|
differential equation (d/dt)x(t) = f(x(t),t) with non-linear f.
|
|
|
|
(3b). covariance estimate propagation model:
|
|
(d/dt)P(t) = F(x(t),t).P(t) + P(t).F(x(t),t)T + Q(t)
|
|
|
|
4. pre-estimate (extrapolation) for t(k+1) system state:
|
|
|
|
can use first order approx:
|
|
x(k+1|k) = x(k) + F(x(tk),tk).(t(k+1) - t(k))
|
|
or multiple timesteps if t(k+1) - t(k) is too large
|
|
|
|
i.e. approximately solving (3a)
|
|
|
|
5. pre-estimate (extrapolation) for t(k+1) error covariance:
|
|
|
|
P(k+1|k) = P(k) + [F(x(tk),tk).P(k) + P(k)F(x(tk),tk)T + Q(tk)].(t(k+1) - t(k))
|
|
or multiple timesteps if t(k+1) - t(k) is too large
|
|
|
|
i.e. approximately solving (3b)
|
|
|
|
6. kalman gain matrix
|
|
K(k+1) = P(k+1|k)
|
|
.H(k+1)T(x(k+1|k))
|
|
.inverse[H(k+1)(x(k+1|k)).P(k+1|k).H(k+1)T(x(k+1|k)) + R(k+1)]
|
|
|
|
7. innovation:
|
|
z(k+1) - h(x(k+1|k), t(k+1))
|
|
|
|
8. corrected state estimate for t(k+1):
|
|
x(k+1) = x(k+1|k) + K(k+1).(z(k+1) - h(k+1)(x(k+1|k))
|
|
|
|
9. corrected error covariance for t(k+1):
|
|
P(k+1) = [I - K(k+1).H(k+1)(x(k+1|k))].P(k+1|k)
|
|
|
|
editorial:
|
|
This step is *sketchy*, to the extend h() is non-linear, linearizing around
|
|
x(k+1|k) may give poor estimate of state covariance
|
|
|
|
10. linearization (1st order term for taylor series of f() around x(t))
|
|
jacobian:
|
|
F(x(t),t) = (df/dx_) evaluated at x_(t)=x(t)
|
|
|
|
H(k+1)(x(k+1|k)) = (dh/dx_) evaluated at x_(t)=x(k+1|k)
|
|
|