?? kalman.c
字號:
/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
* $Id: kalman.c,v 1.5 2002/02/15 18:11:57 tramm Exp $
*
* This is the core Kalman filtering algorithm.
*
* Author: Aaron Kahn, Suresh Kannan, Eric Johnson
* copyright 2001
*
* Merged into the Autopilot project by Trammell Hudson <hudson@swcp.com>
*/
#include <math.h>
#include <string.h>
#include "kalman.h"
/*
* This will perform a Kalman filter Gain, state, and coveriance
* matrix update. What is needed is the linear A matrix, state vector,
* C matrix, P coveriance matrix, measurement vector, and the R matrix.
*
* A(n,n) Linear model
* P(n,n) Coveriance matrix
* X(n,1) State Vector
* C(m,n) Measurement matrix; m=# of measurements, n=# of states
* R(m,n) Measurement weight matrix
* ys(m,1) Measurement vector
* K(n,m) Kalman Gain matrix
*/
void
kalmanUpdate(
Matrix A,
Matrix P,
Vector X,
Matrix C,
Matrix R,
Vector ys,
Matrix K,
int n,
int m
)
{
Matrix E;
Matrix T1;
Matrix T2;
Vector TV1;
Vector TV2;
/* perform E = C*P*C' + R */
MMmult(C,P,T1,m,n,n);
transpose(C,T2,m,n);
MMmult(T1,T2,E,m,n,m);
MMadd(E,R,E,m,m);
/* perform K = P*C'*inv(E). T2 still has C' */
MMmult(P,T2,T1,n,n,m);
inv(E,T2,m);
MMmult(T1,T2,K,n,m,m);
/* perform x = x + K*( ys - yp ) */
MVmult(C,X,TV1,m,n);
VVsub( ys, TV1, TV1, m );
MVmult(K,TV1,TV2,n,m);
VVadd(X,TV2,X,n);
/* perform P = P - K*C*P */
MMmult(K,C,T1,n,m,n);
MMmult(T1,P,T2,n,n,n);
MMsub( P, T2, P, n, n );
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -