Extended kalman算法原理和开源代码阅读

Extended kalman算法原理和开源代码阅读

Extended kalman 理论

理论参考文献：《An Introduction to the Kalman Filter》

EKF的主要步骤：

1、对非线性时变或者非时变系统进行近似线性化，得到离散线性状态空间和观测空间表达式；线性化的过程如下：

原来的非线性时变系统状态方程和观测方程：
$$\begin{array}{rl}{x}_k& =f(x_{k-1},u_{k-1},w_{k-1})\\ z_k& =h(x_k,v_k)\end{array}$$
忽略噪声进行第一步近似：
$$\begin{array}{rl}{\tilde{x}}_k& =f({\hat{x}}_{k-1},u_{k-1},0)\\ {\tilde{z}}_k& =h({\tilde{x}}_k,0)\end{array}$$
雅可比矩阵进行近似线性并且离散化：
$$\begin{array}{rl}{x}_k& \approx {\tilde{x}}_k+A(x_{k-1}-{\hat{x}}_{k-1}+W{w}_{k-1})\\ z_k& \approx {\tilde{z}}_k+H(x_k-{\tilde{x}}_k)+V{v}_k\end{array}$$
其中矩阵$A,W,H,V$是雅可比矩阵近似的时变矩阵，每一步更新都需要改变，计算如下：

2、对于时变系统，每一步推理都需要更新状态矩阵、观测矩阵、噪声协方差矩阵；

3、计算先验误差协方差矩阵；

4、计算增益矩阵；

5、计算状态估计值；

6、计算后验误差协方差矩阵；

开源C代码添加注释

代码参考教授的实现：simondlevy/TinyEKF: Lightweight C/C++ Extended Kalman Filter with Python for prototyping (github.com)

这份代码有优点也有缺点，参考时应该取其精华，去其糟粕：

1、静态分配内存，速度较快，用空间换时间在很多场合是很必要的。但是不能运行时释放，相比于动态分配，失去灵活性。甚至作者连temp变量都用静态的。（其实静态还是动态分配具体看实际需要）

2、兼容不同的平台可以使用宏隔开一个单独的内存分配和释放接口，不一定要用静态内存。

3、还有很多细节问题，例如代码注释规范、将矩阵运算接口独立到一个文件里，而不是和EKF实现文件放在一起等等。

下面是阅读作者的代码做的注释，删除细节实现，保留主要逻辑部分。完整代码看作者的Github。

/*
 * TinyEKF: Extended Kalman Filter for embedded processors
 *
 * Copyright (C) 2015 Simon D. Levy
 *
 * MIT License
 */

/*******************************矩阵基本运算函数**********************************/
//初始化0矩阵
static void zeros(double * a, int m, int n);

//矩阵乘法 /* C <- A * B */
static void mulmat(double * a, double * b, double * c, int arows, int acols, int bcols);

//向量乘法
static void mulvec(double * a, double * x, double * y, int m, int n);

//矩阵转置
static void transpose(double * a, double * at, int m, int n);

//矩阵加法/* A <- A + B */
static void accum(double * a, double * b, int m, int n);

//矩阵加法/* C <- A + B */
static void add(double * a, double * b, double * c, int n);

//矩阵减法 /* C <- A - B */
static void sub(double * a, double * b, double * c, int n);

//矩阵取反
static void negate(double * a, int m, int n);

//矩阵每个元素加1
static void mat_addeye(double * a, int n);

/* Cholesky-decomposition matrix-inversion code, adapated from
   http://jean-pierre.moreau.pagesperso-orange.fr/Cplus/choles_cpp.txt */
//对矩阵求逆矩阵
static int cholsl(double * A, double * a, double * p, int n) ;

/******************************EKF核心代码**********************************/
//核心结构体成员，代表离散系统对象
typedef struct {
    double * x;    /* state vector 状态向量*/

    double * P;  /* prediction error covariance 预测误差协方差矩阵 */
    double * Q;  /* process noise covariance 过程噪声协方差矩阵*/
    double * R;  /* measurement error covariance 观测误差协方差矩阵*/

    double * G;  /* Kalman gain; a.k.a. K 卡尔曼增益*/

    double * F;  /* Jacobian of process model 过程模型雅可比矩阵*/
    double * H;  /* Jacobian of measurement model 观测模型雅可比矩阵*/

    double * Ht; /* transpose of measurement Jacobian 观测模型雅可比矩阵转置*/
    double * Ft; /* transpose of process Jacobian 过程模型雅可比矩阵转置*/
    double * Pp; /* P, post-prediction, pre-update 后验估计矩阵P，临时用*/

    double * fx;  /* output of user defined f() state-transition function系统非线性模型函数，矩阵 */
    double * hx;  /* output of user defined h() measurement function 非线性模型观测函数，非线性矩阵*/
} ekf_t;

//矩阵指针指向对应的内存地址，根据作者的说法是因为应用于各种平台，而各种平台的内存申请接口不同，所以使用静态分配内存适配不同的平台。
static void unpack(void * v, ekf_t * ekf, int n, int m);

//初始化ekf结构体，初值和内存指针
void ekf_init(void * v, int n, int m);


//核心代码，EKF每一步迭代的代码
int ekf_step(void * v, double * z)
{        
    /*1、每一步都要先更新*ekf结构体/

    /*2、计算误差协方差矩阵（临时）： P_k = F_{k-1} P_{k-1} F^T_{k-1} + Q_{k-1} */
    mulmat(ekf.F, ekf.P, ekf.tmp0, n, n, n);
    transpose(ekf.F, ekf.Ft, n, n);
    mulmat(ekf.tmp0, ekf.Ft, ekf.Pp, n, n, n);
    accum(ekf.Pp, ekf.Q, n, n);
    /*3、计算增益矩阵： G_k = P_k H^T_k (H_k P_k H^T_k + R)^{-1} */
    transpose(ekf.H, ekf.Ht, m, n);
    mulmat(ekf.Pp, ekf.Ht, ekf.tmp1, n, n, m);
    mulmat(ekf.H, ekf.Pp, ekf.tmp2, m, n, n);
    mulmat(ekf.tmp2, ekf.Ht, ekf.tmp3, m, n, m);
    accum(ekf.tmp3, ekf.R, m, m);
    if (cholsl(ekf.tmp3, ekf.tmp4, ekf.tmp5, m)) return 1;
    mulmat(ekf.tmp1, ekf.tmp4, ekf.G, n, m, m);

    /*4、计算状态变量估计值： \hat{x}_k = \hat{x_k} + G_k(z_k - h(\hat{x}_k)) */
    sub(z, ekf.hx, ekf.tmp5, m);
    mulvec(ekf.G, ekf.tmp5, ekf.tmp2, n, m);
    add(ekf.fx, ekf.tmp2, ekf.x, n);
    
    /*5、计算误差协方差矩阵： P_k = (I - G_k H_k) P_k */
    mulmat(ekf.G, ekf.H, ekf.tmp0, n, m, n);
    negate(ekf.tmp0, n, n);
    mat_addeye(ekf.tmp0, n);
    mulmat(ekf.tmp0, ekf.Pp, ekf.P, n, n, n);
}

用于GPS滤波的步骤：

作者给出一个应用于GPS滤波的示例，删掉细节代码，保留主要逻辑，完整文件看原作者的github：

/* gps_ekf: TinyEKF test case using You Chong's GPS example:
 * 
 *   http://www.mathworks.com/matlabcentral/fileexchange/31487-extended-kalman-filter-ekf--for-gps
 * 
 * Reads file gps.csv of satellite data and writes file ekf.csv of mean-subtracted estimated positions.
 *
 *
 * References:
 *
 * 1. R G Brown, P Y C Hwang, "Introduction to random signals and applied 
 * Kalman filtering : with MATLAB exercises and solutions",1996
 *
 * 2. Pratap Misra, Per Enge, "Global Positioning System Signals, 
 * Measurements, and Performance(Second Edition)",2006
 * 
 * Copyright (C) 2015 Simon D. Levy
 *
 * MIT License
 */

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <strings.h>
#include <math.h>

#include "tinyekf_config.h"
#include "tiny_ekf.h"

// positioning interval
static const double T = 1;

//工具函数，填充矩阵
static void blkfill(ekf_t * ekf, const double * a, int off);

//初始化噪声协方差矩阵等
static void init(ekf_t * ekf);

//每次都更新fx F hx H矩阵，因为是时变系统
static void model(ekf_t * ekf, double SV[4][3]);

//读取一行数据
static void readline(char * line, FILE * fp);

//读取数据
static void readdata(FILE * fp, double SV_Pos[4][3], double SV_Rho[4]);

//读取数据
static void skipline(FILE * fp);


int main(int argc, char ** argv)
{    
    //初始化

    //打开文件
    FILE * ifp = fopen("gps.csv", "r");

    //读取文件，观测值
    skipline(ifp);
    double SV_Pos[4][3];
    double SV_Rho[4];
    double Pos_KF[25][3];
    const char * OUTFILE = "ekf.csv";
    FILE * ofp = fopen(OUTFILE, "w");
    fprintf(ofp, "X,Y,Z\n");
    int j, k;

    // 不断一步步使用EKF进行估计当前GPS位置 速度 时钟偏差
    for (j=0; j<25; ++j) {

        //读取观测值
        readdata(ifp, SV_Pos, SV_Rho);

        
		//每次都更新fx F hx H矩阵，因为是时变系统
        model(&ekf, SV_Pos);

        //EKF单步推理
        ekf_step(&ekf, SV_Rho);

        // 保留位置估计，不管速度
        for (k=0; k<3; ++k)
            Pos_KF[j][k] = ekf.x[2*k];
    }

    //计算位置均值
    double mean_Pos_KF[3] = {0, 0, 0};
    for (j=0; j<25; ++j) 
        for (k=0; k<3; ++k)
            mean_Pos_KF[k] += Pos_KF[j][k];
    for (k=0; k<3; ++k)
        mean_Pos_KF[k] /= 25;


    // 计算均方差，评估离散程度
    for (j=0; j<25; ++j) {
        fprintf(ofp, "%f,%f,%f\n", 
                Pos_KF[j][0]-mean_Pos_KF[0], Pos_KF[j][1]-mean_Pos_KF[1], Pos_KF[j][2]-mean_Pos_KF[2]);
        printf("%f %f %f\n", Pos_KF[j][0], Pos_KF[j][1], Pos_KF[j][2]);
    }
    
    // Done!
    fclose(ifp);
    fclose(ofp);
    printf("Wrote file %s\n", OUTFILE);
    return 0;
}

参考：

extended Kalman Filter(EKF) for GPS - File Exchange - MATLAB Central (mathworks.cn)

simondlevy/TinyEKF: Lightweight C/C++ Extended Kalman Filter with Python for prototyping (github.com)

An Introduction to the Kalman Filter