简介
本小节,使用C语言来实现卡尔曼滤波。
测试部分待补充。
准备工作
时间更新方程
x
^
k
ˉ
=
A
x
^
k
−
1
+
B
u
k
−
1
①
{\hat{x}}_{\bar{k}}=A{\hat{x}}_{{k-1}}+Bu_{k-1} ~~~~~~~~~~~~~~~ ①
x^kˉ=Ax^k−1+Buk−1 ①
P
k
ˉ
=
A
P
k
−
1
A
T
+
Q
②
P_{\bar{k}}=AP_{k-1}A^T+Q ~~~~~~~~~~~~~~~ ②
Pkˉ=APk−1AT+Q ②
状态更新方程
K
k
=
P
k
ˉ
H
T
H
P
k
ˉ
H
T
+
R
③
K_k=\frac {P_{\bar{k}}H^T} {HP_{\bar{k}}H^T+R}~~~~~~~~~~~~~~~ ③
Kk=HPkˉHT+RPkˉHT ③
x
^
k
=
x
^
k
ˉ
+
K
k
(
z
k
−
H
x
^
k
ˉ
)
④
{\hat{x}}_{k}={\hat{x}}_{\bar{k}}+K_k(z_k-H{\hat{x}}_{\bar{k}})~~~~~~~~~~~~~~~ ④
x^k=x^kˉ+Kk(zk−Hx^kˉ) ④
P
k
=
(
I
−
K
k
H
)
P
k
ˉ
⑤
P_k=(I-K_kH)P_{\bar{k}}~~~~~~~~~~~~~~~ ⑤
Pk=(I−KkH)Pkˉ ⑤
变量对应关系
变量 | 值 | 说明 |
---|---|---|
n n n | 状态量个数 | |
m m m | 观测量个数 | |
x ^ k ˉ {\hat{x}}_{\bar{k}} x^kˉ | x n × 1 {x \atop n×1 } n×1x | 外部输入 |
P k ˉ P_{\bar{k}} Pkˉ | x n × 1 {x \atop n×1 } n×1x | 外部输入 |
H H H | H m × n {H \atop m×n } m×nH | 外部输入 |
v = z k − H x ^ k ˉ v=z_k-H{\hat{x}}_{\bar{k}} v=zk−Hx^kˉ | v m × 1 {v \atop m×1 } m×1v | 外部输入 |
R R R | R m × m R \atop m×m m×mR | 外部输入 |
x ^ k {\hat{x}}_{k} x^k | x p n × 1 {xp \atop n×1 } n×1xp | 输出 |
P k P_k Pk | P p n × n {Pp \atop n×n } n×nPp | 输出 |
K k K_k Kk | K n × m K \atop n×m n×mK | 临时变量 |
F = P k ˉ H T F=P_{\bar{k}}H^T F=PkˉHT | F n × m F \atop n×m n×mF | 临时变量 |
S = H P k ˉ H T + R S=HP_{\bar{k}}H^T+R S=HPkˉHT+R | S m × m S \atop m×m m×mS | 临时变量 |
实现
函数定义
int filter_kalman(const double *x, const double *P, const double *H,
const double *v, const double *R, int n, int m,
double *xp, double *Pp)
临时变量
double *F = mat(n, m), *S = mat(m, m), *K = mat(n, m), *I = eye(n);
计算F
matmul("NT", n, m, n, 1.0, P, H, 0.0, F); /* F = P*H' */
计算S
matcpy(S, R, m, m);
matmul("NN", m, m, n, 1.0, H, F, 1.0, S); /* S = H*P*H'+R =HF+R */
计算K
matinv(S, m)
matmul("NN", n, m, m, 1.0, F, S, 0.0, K); /* K = F/S */
计算xp
matcpy(xp, x, n, 1);
matmul("NN", n, 1, m, 1.0, K, v, 1.0, xp); /* xp = x+K*v */
计算Pp
matmul("NN", n, n, m, -1.0, K, H, 1.0, I);
matmul("NN", n, n, n, 1.0, I, P, 0.0, Pp);/* Pp = (I-K*H)*P */
完整实现
int filter_kalman(const double *x, const double *P, const double *H,
const double *v, const double *R, int n, int m,
double *xp, double *Pp)
{
double *F = mat(n, m), *S = mat(m, m), *K = mat(n, m), *I = eye(n);
int info;
matmul("NT", n, m, n, 1.0, P, H, 0.0, F); /* F = P*H' */
matcpy(S, R, m, m);
matmul("NN", m, m, n, 1.0, H, F, 1.0, S); /* S = H*P*H'+R =HF+R */
if (!(info = matinv(S, m))) {
matmul("NN", n, m, m, 1.0, F, S, 0.0, K); /* K = F/S */
matcpy(xp, x, n, 1);
matmul("NN", n, 1, m, 1.0, K, v, 1.0, xp); /* xp = x+K*v */
matmul("NN", n, n, m, -1.0, K, H, 1.0, I);
matmul("NN", n, n, n, 1.0, I, P, 0.0, Pp);/* Pp = (I-K*H)*P */
}
free(F); free(S); free(K); free(I);
return info;
}