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
|
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "kalman_filter.h"
void KalmanPredict(
KAL_VEC(xhat_k_km1), /* OUTPUT: (S) Predicted state at time 'k' */
KAL_MAT(P_k_km1), /* OUTPUT: (S x S) Predicted covariance at time 'k' */
KAL_MAT(P_km1_km1), /* INPUT: (S x S) Updated covariance from time 'k-1' */
KAL_VEC(xhat_km1_km1), /* INPUT: (S) Updated state from time 'k-1' */
KAL_MAT(F_k), /* INPUT: (S x S) State transition model */
KAL_MAT(B_k), /* INPUT: (S x U) Control input model */
KAL_VEC(u_k), /* INPUT: (U) Control vector */
KAL_MAT(Q_k), /* INPUT: (S x S) Covariance of process noise */
int S, /* INPUT: Number of dimensions in state vector */
int U) /* INPUT: Size of control input vector */
{
KAL_MAT(F_k_tran);
KAL_MAT(F_k__P_km1_km1);
// Predicted state: xhat_k_km1 = Fk * xhat_km1_km1 + Bk * uk
MUL(F_k, xhat_km1_km1, xhat_k_km1, S,S,1);
// Predicted covar: P_k_km1 = Fk * P_km1_km1 * Fk' + Qk
MUL(F_k, P_km1_km1, F_k__P_km1_km1, S, S, S);
TRANSP(F_k, F_k_tran, S, S);
MULADD(F_k__P_km1_km1, F_k_tran, Q_k, P_k_km1, S, S, S);
}
void KalmanUpdate(
KAL_VEC(xhat_k_k), /* (S) OUTPUT: Updated state at time 'k' */
KAL_MAT(P_k_k), /* (S x S) OUTPUT: Updated covariance at time 'k' */
KAL_VEC(xhat_k_km1), /* (S) INPUT: Predicted state at time 'k' */
KAL_MAT(P_k_km1), /* (S x S) INPUT: Predicted covariance at time 'k' */
KAL_VEC(z_k), /* (B) INPUT: Observation vector */
KAL_MAT(H_k), /* (B x S) INPUT: Observational model */
KAL_MAT(R_k), /* (S x S) INPUT: Covariance of observational noise */
int B, /* INPUT: Number of observations in observation vector */
int S) /* INPUT: Number of measurements in the state vector */
{
// UPDATE PHASE
// Measurement residual: yhat_k = zk - Hk * xhat_k_km1
GMULADD(H_k,xhat_k_km1,z_k,D,-1.0f,1.0f,S,S,1);
// Residual covariance: S_k = H_k * P_k_km1 * H_k' + R_k
// Optimal Kalman gain: K_k = P_k_km1 * H_k' * inv(S_k)
// Updated state esti: xhat_k_k = xhat_k_km1 + K_k * yhat_k
// Updated covariance: P_k_k = (I - K_k * H_k) * P_k_km1
}
|
