卡尔曼滤波---C语言实现（二）

简介

本小节，使用C语言来实现卡尔曼滤波。
测试部分待补充。

准备工作

时间更新方程

${\hat{x}}_{\bar{k}}=A{\hat{x}}_{{k-1}}+Bu_{k-1} ~~~~~~~~~~~~~~~ ①$
$P_{\bar{k}}=AP_{k-1}A^T+Q ~~~~~~~~~~~~~~~ ②$

状态更新方程

$K_k=\frac {P_{\bar{k}}H^T} {HP_{\bar{k}}H^T+R}~~~~~~~~~~~~~~~ ③$
${\hat{x}}_{k}={\hat{x}}_{\bar{k}}+K_k(z_k-H{\hat{x}}_{\bar{k}})~~~~~~~~~~~~~~~ ④$
$P_k=（I-K_kH）P_{\bar{k}}~~~~~~~~~~~~~~~ ⑤$

变量对应关系

变量	值	说明
$n$		状态量个数
$m$		观测量个数
${\hat{x}}_{\bar{k}}$	${x \atop n×1 }$	外部输入
$P_{\bar{k}}$	${x \atop n×1 }$	外部输入
$H$	${H \atop m×n }$	外部输入
$v=z_k-H{\hat{x}}_{\bar{k}}$	${v \atop m×1 }$	外部输入
$R$	$R \atop m×m$	外部输入
${\hat{x}}_{k}$	${xp \atop n×1 }$	输出
$P_k$	${Pp \atop n×n }$	输出
$K_k$	$K \atop n×m$	临时变量
$F=P_{\bar{k}}H^T$	$F \atop n×m$	临时变量
$S=HP_{\bar{k}}H^T+R$	$S \atop m×m$	临时变量

实现

函数定义

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;
}

测试（暂无，之后补充）