卡爾曼濾波---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;
}

測試（暫無，之後補充）