卡尔曼滤波算法

这里就不详细讲解该算法，我觉得比较好的讲解，可以参考文章：
【工程师学算法】工程常用算法（二）—— 卡尔曼滤波(Kalman Filter)

随机数的产生

均匀分布随机数的产生

均匀分布的概率密度函数为：
$$f(x)=\{\begin{array}{ll}\frac{1}{b-a},& a\le x\le b\\ 0,& 其他\end{array}$$
通常用 $U(a,b)$表示。均匀分布的均值为$(a+b)/2$，方差为$(a-b{)}^2/12$。产生均匀分布随机数的方法如下：

• 由给定的初值$x_0$，用混合同余法：$$\{\begin{array}{l}{x}_i=(a{x}_{i-1}+c)modM\\ y_i=x_i/M\end{array}$$
• 通过变换$z_i=a+(b-a)y_i$产生$(a,b)$区间上的随机数$z_i$。
  C语言代码如下：

typedef float DistriType;
DistriType Uniform(DistriType a,DistriType b,long int *seed)
{
    DistriType t;
    *seed = 2045 * (*seed) + 1;
    *seed = *seed - (*seed / 1048576) * 1048576;
    t = (*seed) / 1048576.0;
    t = a + (b - a) * t;
    return t;
}

正态分布的随机数

正态分布的概率密度函数为
$$f(x)=\frac{1}{\sqrt{2\pi }\sigma }{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$$
通常用$N(\mu ,{\sigma }^2)$表示。式中$\mu$为均值，${\sigma }^2$为方差。正态分布也称高斯分布。
设$r_1,r_2,...,r_n$为$(0,1)$上的$n$个相互独立的均匀分布的随机数，由于$E(r_i)=1/2,D(r_i)=1/12$，根据中心极限定理可知，当$n$充分大时：
$$x=\sqrt{\frac{12}{n}}(\sum _{i=1}^n{r}_i-\frac{n}{2})$$近似于正态分布$N(0,1)$。通常取$n=12$，此时有
$$x=\sum _{i=1}^{12}{r}_i-6$$
最后，再通过变换$y=\mu +\sigma x$，便可得到均值为$\mu$，方差为${\sigma }^2$的正态分布随机数。C语言实现为：

DistriType Gauss(DistriType mean, DistriType sigma, long int *s)
{
    int i;
    DistriType x,y;
    for(x = 0, i = 0; i < 12; i++)
    {
       x += Uniform(0.0, 1.0, s);
    }
    x -= 6.0;
    y = mean + x * sigma;
    return y;
}

矩阵的C语言实现

新建一个Mat.h文档，内容如下：

#ifndef MAT_H
#define MAT_H

typedef double MatEleType;
typedef struct
{
    MatEleType **mat;
    int  row;
    int  col;
}Matrix,*MatPtr;

extern void PrintMatrix(MatPtr T);
extern MatPtr CreateMatrix( int row, int col) ;
extern void DestroyMatrix(MatPtr T);
extern void InitMatrix(MatPtr T, MatEleType arr[]);
extern void SetMatrix(MatPtr T, ...);
extern MatPtr CopyMatrix(MatPtr src);
extern MatPtr AddMatrix(MatPtr A, MatPtr B);
extern MatPtr SubMatrix(MatPtr A, MatPtr B);
extern MatPtr TransMatrix(MatPtr A);
extern MatPtr ProductMatrix(MatPtr A, MatPtr B);
extern MatPtr InvertMatrix(MatPtr input);
extern MatPtr IdentitySubMatrix(MatPtr T);

新建一个Mat.c文档，内容如下：

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdarg.h>
#include "Mat.h"

void PrintMatrix(Matrix *T)
{
    int i,j;
    if (T == NULL)
    {
        printf("the matrix is null!!!\n");
        return;
    }
    printf("ans=\n");
    for (i = 0; i < T->row; i++)
    {
        printf("\t");
        for (j = 0; j < T->col; j++)
        {
            printf("%f\t", T->mat[i][j]);
        }
        printf("\n");
    }
    printf("\n");
}

MatPtr CreateMatrix( int row, int col)
{
    int i,j;
    MatPtr T = NULL;
    
    T = (MatPtr) malloc(sizeof(Matrix));
    if (T == NULL)
    {
        return NULL;
    }
    
    T->mat = (MatEleType **)malloc(row * sizeof(MatEleType *));
    if (T->mat == NULL)
    {
        free(T);
        return NULL;
    }
    for (i = 0; i < row; i++)
    {
        T->mat[i] = (MatEleType *)malloc(col * sizeof(MatEleType));
        if (T->mat[i] == NULL)
        {
            for (j = 0; j < i; j++)
            {
                free(T->mat[j]);
            }
            free(T->mat);
            free(T);
            return NULL;
        }
    }
    T->row = row;
    T->col = col;
    
    return T;
}

void DestroyMatrix(MatPtr T)
{
    int i;
    for (i = 0; i < T->row; i++)
    {
        free(T->mat[i]);
    }
    free(T->mat);
    free(T);
}

void InitMatrix(MatPtr T, MatEleType arr[])
{
    int i, j;
    for (i = 0; i < T->row; i++)
    {
        for (j = 0; j < T->col; j++)
        {
            T->mat[i][j] = arr[ i * T->col + j];
        }
    }
}

void SetMatrix(MatPtr T, ...)
{
    va_list ap;
    int i, j;
    va_start(ap, T);
    for (i = 0; i < T->row; i++)
    {
        for (j = 0; j < T->col; j++)
        {
            T->mat[i][j] = va_arg(ap, double);
        }
    }
    va_end(ap);
}

MatPtr CopyMatrix(MatPtr src)
{
    int i, j;
    MatPtr T = NULL;
    if (src == NULL)
    {
        return NULL;
    }
    T = CreateMatrix(src->row, src->col);
    if (T == NULL)
    {
        return NULL;
    }
    for (i = 0; i < src->row; i++)
    {
        for (j = 0; j < src->col; j++)
        {
            T->mat[i][j] = src->mat[i][j];
        }
    }
    return T;
}

MatPtr TransMatrix(MatPtr A)
{
    int i, j;
    MatPtr T = NULL;
    if (A == NULL)
    {
        return NULL;
    }
    T = CreateMatrix(A->col, A->row);
    if (T == NULL)
    {
        return NULL;
    }
    for (i = 0; i < A->row; i++)
    {
        for (j = 0; j < A->col; j++)
        {
            T->mat[j][i] = A->mat[i][j];
        }
    }
    return T;
}

MatPtr AddMatrix(MatPtr A, MatPtr B)
{
    int i, j;
    MatPtr T = NULL;
    if (A == NULL || B == NULL)
    {
        return NULL;
    }
    if ( (A->row != B->row) || (A->col != B->col) )
    {
        return NULL;
    }
    T = CreateMatrix(A->row, A->col);
    if (T == NULL)
    {
        return NULL;
    }
    for (i = 0; i < A->row; i++)
    {
        for (j = 0; j < A->col; j++)
        {
            T->mat[i][j] = A->mat[i][j] + B->mat[i][j];
        }
    }
    return T;
}

MatPtr SubMatrix(MatPtr A, MatPtr B)
{
    int i, j;
    MatPtr T = NULL;
    if (A == NULL || B == NULL)
    {
        return NULL;
    }
    if ( (A->row != B->row) || (A->col != B->col) )
    {
        return NULL;
    }
    T = CreateMatrix(A->row, A->col);
    if (T == NULL)
    {
        return NULL;
    }
    for (i = 0; i < A->row; i++)
    {
        for (j = 0; j < A->col; j++)
        {
            T->mat[i][j] = A->mat[i][j] - B->mat[i][j];
        }
    }
    return T;
}

MatPtr ProductMatrix(MatPtr A, MatPtr B)
{
    int i, j, k;
    MatPtr T = NULL;
    if (A == NULL || B == NULL)
    {
        return NULL;
    }
    if ( (A->col != B->row) )
    {
        return NULL;
    }
    T = CreateMatrix(A->row, B->col);
    if (T == NULL)
    {
        return NULL;
    }
    for (i = 0; i < A->row; i++)
    {
        for (j = 0; j < B->col; j++)
        {
            T->mat[i][j] = 0.0;
            for (k = 0; k < A->col; k++)
            {
                T->mat[i][j] += A->mat[i][k] * B->mat[k][j];
            }
        }
    }
    return T;
}

MatPtr IdentitySubMatrix(MatPtr T)
{
 int i, j;
 MatPtr tmp;
    if (T->row != T->row)
    {
        return NULL;
    }
    tmp = CreateMatrix(T->row, T->col);
    for (i = 0; i < T->row; i++)
    {
        for (j = 0; j < T->col; j++)
        {
            if (i == j)
            {
                tmp->mat[i][j] = 1.0 - T->mat[i][j];
            }
            else
            {
                tmp->mat[i][j] = 0.0 - T->mat[i][j];
            }
        }
    }
    return tmp;
}

static void identityMatrix(MatPtr T)
{
    int i, j;
    if (T->row != T->col)
    {
        return;
    }
    for (i = 0; i < T->row; i++)
    {
        for (j = 0; j < T->col; j++)
        {
            if (i == j)
            {
                T->mat[i][j] = 1.0;
            }
            else
            {
                T->mat[i][j] = 0.0;
            }
        }
    }
}

/* I - T */
static void SubMatrixFromIdentity(MatPtr T)
{
    int i, j;
    if (T->row != T->row)
    {
        return;
    }
    for (i = 0; i < T->row; i++)
    {
        for (j = 0; j < T->col; j++)
        {
            if (i == j)
            {
                T->mat[i][j] = 1.0 - T->mat[i][j];
            }
            else
            {
                T->mat[i][j] = 0.0 - T->mat[i][j];
            }
        }
    }
}

/*
  this function is to determine whether the two matrix are equal or not
    if A==B then the returned vaule is 1,else the returned vaule is 0
*/
static int isEqualMatrix(MatPtr A, MatPtr B, MatEleType tolerance)
{
    int i, j;
    if (A == NULL || B == NULL)
    {
        return 0;
    }
    if ( (A->col != B->col) || (A->row != B->row) )
    {
        return 0;
    }
    for (i = 0; i < A->row; i++)
    {
        for (j = 0; j < A->col; j++)
        {
            if (abs(A->mat[i][j] - B->mat[i][j]) > tolerance)
            {
                return 0;
            }
        }
    }
    return 1;
}

/*
 * A = a*A, a is a scalar
*/
static void scaleMatrix(MatPtr T, MatEleType scalar)
{
    int i, j;
    if (T == NULL)
    {
        return;
    }
    for (i = 0; i < T->row; i++)
    {
        for (j = 0; j < T->col; j++)
        {
            T->mat[i][j] *= scalar;
        }
    }
}

/*
 * exchange two rows of the matrix
*/
static void swapRowsMatrix(MatPtr T, int r1, int r2)
{
    MatEleType *tmp;
    if (r1 == r2)
    {
        return;
    }
    tmp = T->mat[r1];
    T->mat[r1] = T->mat[r2];
    T->mat[r2] = tmp;
}

/*
 * the specified matrix's row product a scalar
*/
static void scaleRow(MatPtr T, int r, MatEleType scalar)
{
    int i;
    for (i = 0; i < T->col; i++)
    {
        T->mat[r][i] *= scalar;
    }
}

/*
 * Add scalar*row r2 to row r1.
*/
static void shearRow(MatPtr T, int r1, int r2, MatEleType scalar)
{
    int i;
    if (r1 == r2)
    {
        return;
    }
    for (i = 0; i < T->col; i++)
    {
        T->mat[r1][i] += scalar * T->mat[r2][i];
    }
}

/* Uses Gauss-Jordan elimination.
   The elimination procedure works by applying elementary row
   operations to our input matrix until the input matrix is reduced to
   the identity matrix.
   Simultaneously, we apply the same elementary row operations to a
   separate identity matrix to produce the inverse matrix.
   If this makes no sense, read wikipedia on Gauss-Jordan elimination.
   This is not the fastest way to invert matrices, so this is quite
   possibly the bottleneck. */
MatPtr InvertMatrix(MatPtr input)
{
    int i, j, r;
    MatEleType scalar;
    MatEleType shearNeeded;
    MatPtr output = NULL;
    MatPtr tmp = NULL;
    MatEleType eps = 0.0000001;
    if (input->row != input->col)
    {
        return NULL;
    }
    output = CreateMatrix(input->row, input->col);
    tmp = CopyMatrix(input);
    if (output == NULL || tmp == NULL)
    {
        return NULL;
    }
    identityMatrix(output);
    /* Convert input to the identity matrix via elementary row operations.
         The ith pass through this loop turns the element at i,i to a 1
         and turns all other elements in column i to a 0. */
    for (i = 0 ; i < tmp->row; i++)
    {
        if (abs(tmp->mat[i][i]) < eps)
        {
            for (r = i + 1; r < tmp->row; r++)
            {
                if (abs(tmp->mat[r][i]) > eps)
                {
                    /* We must swap rows to get a nonzero diagonal element. */
                    break;
                }
            }
            if (r == tmp->row)
            {
                /* Every remaining element in this column is zero, so this
                       matrix cannot be inverted. */
                printf("cannot be inverted!!\n");
                printf("the matrix is :\n");
                PrintMatrix(input);
                return NULL;
            }
            swapRowsMatrix(tmp, i, r);
            swapRowsMatrix(output, i, r);
        }
        /* Scale this row to ensure a 1 along the diagonal.
               We might need to worry about overflow from a huge scalar here. */
        scalar = 1.0 / tmp->mat[i][i];
        scaleRow(tmp, i, scalar);
        scaleRow(output, i, scalar);
        /* Zero out the other elements in this column. */
        for (j = 0; j < tmp->row; j++)
        {
            if (i == j)
            {
                continue;
            }
            shearNeeded = -tmp->mat[j][i];
            shearRow(tmp, j, i, shearNeeded);
            shearRow(output, j, i, shearNeeded);
        }
    }
    DestroyMatrix(tmp);
    return output;
}

卡尔曼滤波测试

卡尔曼滤波测试程序如下所示，该程序采用【工程师学算法】工程常用算法（二）—— 卡尔曼滤波(Kalman Filter)中一个例子，即滑轨上的小车的例子。其中两个传感器分别用于测量位置和速度。

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include "Mat.h"
#include "distribution.h"

MatPtr ZtGenerate(MatPtr A, MatPtr B,MatPtr R, MatPtr Ut,long int *seedx, long int *seedv)
{
    MatPtr Zt,Rdata,tmp1,tmp2;
    static MatPtr Xt = NULL;
    Rdata = CreateMatrix(2,1)
    Rdata->mat[0][0] = Gauss(0.0, sqrt(R->mat[0][0]), seedx);
    Rdata->mat[1][0] = Gauss(0.0, sqrt(R->mat[1][1]), seedv);
    if(Xt == NULL)
    {
        Xt = CreateMatrix(2,1);
        SetMatrix(Xt, 0.0, 0.0);;
    }
    tmp1 = ProductMatrix(A, Xt);
    tmp2 = ProductMatrix(B, Ut);
    DestroyMatrix(Xt);
    Xt = AddMatrix(tmp1, tmp2);
    DestroyMatrix(tmp1);
    DestroyMatrix(tmp2);
    Zt = AddMatrix(Xt, Rdata);
    return Zt;
}

void KalmanFilter(void)
{
    int cnt = 0;
    static long int seedx0,seedv0,seedx1,seedv1;
    MatPtr tmp1,tmp2,tmp3,Kt;
    MatPtr A,B,W,Xt,Ut,Pt,Q,R,Zt;
    MatEleType period = 0.1;
    FILE *fw = NULL;
    
    A = CreateMatrix(2,2);
    SetMatrix(A, 1.0, period, 0.0, 1.0);
    B = CreateMatrix(2,1);
    SetMatrix(B, 0.5*period*period, period);
    W = CreateMatrix(2,1);
    SetMatrix(W, 0.1, 0.1); 
    Xt = CreateMatrix(2,1);
    SetMatrix(Xt, 0.0, 0.0);
    Ut = CreateMatrix(1,1);
    SetMatrix(Ut, 1.0);
    Pt = CreateMatrix(2,2);
    SetMatrix(Pt, 1.0, 0.0, 0.0, 1.0);
    Q = CreateMatrix(2,2);
    SetMatrix(Q, 4.0, 0.0, 0.0, 3.0);
    R = CreateMatrix(2,2);
    SetMatrix(R, 3.0, 0.0, 0.0, 2.25); 
    fw = fopen("F:/dev/kalmanFilter/gauss.txt","w");
    if(fw == NULL)
    {
        return;
    }
    srand( (unsigned)time(NULL) );
    seedx0 = rand();
    seedv0 = rand();
    seedx1 = rand();
    seedv1 = rand();
    while(1)
    {
        printf("cnt=%d\n",cnt);
        // 公式1 
        W->mat[0][0] = Gauss(0.0, 2, &seedx0);
        W->mat[1][0] = Gauss(0.0, 1.5, &seedv0); 
        tmp1 = ProductMatrix(A, Xt);
        tmp2 = ProductMatrix(B, Ut);
        tmp3 = AddMatrix(tmp1, tmp2);
        DestroyMatrix(tmp1);
        DestroyMatrix(tmp2);
        DestroyMatrix(Xt);
        Xt = AddMatrix(tmp3, W); 
        // 公式2
        tmp1 = ProductMatrix(A,Pt);
        tmp2 = TransMatrix(A);
        tmp3 = ProductMatrix(tmp1,tmp2);
        DestroyMatrix(tmp1);
        DestroyMatrix(tmp2);
        DestroyMatrix(Pt);
        Pt = AddMatrix(tmp3, Q); 
        // 公式3 
        tmp1 = AddMatrix(Pt,R);
        tmp2 = InvertMatrix(tmp1);
        Kt = ProductMatrix(Pt, tmp2);
        DestroyMatrix(tmp1);
        DestroyMatrix(tmp2);
        // 公式4
        Zt = ZtGenerate(A, B, R,Ut,&seedx1,&seedv1);
        tmp1 = SubMatrix(Zt, Xt);
        DestroyMatrix(Zt);
        tmp2 = ProductMatrix(Kt, tmp1);
        tmp3 = AddMatrix(Xt, tmp2);
        DestroyMatrix(Xt);
        Xt = CopyMatrix(tmp3);
        DestroyMatrix(tmp1);
        DestroyMatrix(tmp2);
        DestroyMatrix(tmp3);
        // 公式5
        tmp1 = IdentitySubMatrix(Kt);
        tmp2 = ProductMatrix(tmp1, Pt);
        DestroyMatrix(Pt);
        Pt = CopyMatrix(tmp2);
        DestroyMatrix(tmp1);
        DestroyMatrix(tmp2);
        fprintf(fw,"%f\t%f\n",Xt->mat[0][0],Xt->mat[1][0]);
        if(cnt++>1000) break;  
    }
    DestroyMatrix(A);
    DestroyMatrix(B);
    DestroyMatrix(Xt);
    DestroyMatrix(Ut);
    DestroyMatrix(W);
    fclose(fw);
}