卡尔曼滤波算法

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

随机数的产生

均匀分布随机数的产生

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

由给定的初值 x 0 x_0 x0，用混合同余法： { x i = ( a x i − 1 + c ) m o d M y i = x i / M \begin{cases}x_i =(ax_{i-1}+c) mod M \\y_i={x_i}/{M} \end{cases} {xi=(axi−1+c)modMyi=xi/M
通过变换 z i = a + ( b − a ) y i z_i=a+(b-a)y_i zi=a+(b−a)yi产生 ( a , b ) (a,b) (a,b)区间上的随机数 z i z_i zi。
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 ) = 1 2 π σ e − ( x − μ ) 2 2 σ 2 f(x)=\frac{1}{\sqrt{2\pi}\sigma}e^{-\frac{(x-\mu)^2}{2\sigma^2}} f(x)=2π σ1e−2σ2(x−μ)2
通常用 N ( μ , σ 2 ) N(\mu,\sigma^2) N(μ,σ2)表示。式中 μ \mu μ为均值， σ 2 \sigma^2 σ2为方差。正态分布也称高斯分布。
设 r 1 , r 2 , . . . , r n r_1,r_2,...,r_n r1,r2,...,rn为 ( 0 , 1 ) (0,1) (0,1)上的 n n n个相互独立的均匀分布的随机数，由于 E ( r i ) = 1 / 2 , D ( r i ) = 1 / 12 E(r_i)=1/2,D(r_i)=1/12 E(ri)=1/2,D(ri)=1/12，根据中心极限定理可知，当 n n n充分大时：
x = 12 n ( ∑ i = 1 n r i − n 2 ) x=\sqrt{\frac{12}{n}}(\sum^n_{i=1}r_i-\frac{n}{2}) x=n12 (i=1∑nri−2n)近似于正态分布 N ( 0 , 1 ) N(0,1) N(0,1)。通常取 n = 12 n=12 n=12，此时有
x = ∑ i = 1 12 r i − 6 x=\sum^{12}_{i=1}r_i-6 x=i=1∑12ri−6
最后，再通过变换 y = μ + σ x y=\mu+\sigma x y=μ+σx，便可得到均值为 μ \mu μ，方差为 σ 2 \sigma^2 σ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_Htypedef 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 notif 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 rowoperations to our input matrix until the input matrix is reduced tothe identity matrix.Simultaneously, we apply the same elementary row operations to aseparate 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 quitepossibly 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 1and 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 thismatrix 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); // 公式2tmp1 = 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);// 公式4Zt = 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);// 公式5tmp1 = 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);
}

卡尔曼滤波C语言实现（矩阵版）相关推荐

【飞控理论】从零开始学习Kalman Filters之四：卡尔曼滤波C语言代码实现
文章目录前言学习目录 1.卡尔曼线性滤波的五条黄金公式 2.陀螺仪的原始数据 3.C语言源码分析附录 1.矩阵乘法 2.协方差矩阵 3.单位矩阵前言前面的文章系统介绍了卡尔曼滤波算法的数 ...
俄罗斯方块-C语言-详注版
代码地址如下: http://www.demodashi.com/demo/14818.html 俄罗斯方块-C语言-详注版概述本文详述了C语言版俄罗斯方块游戏的原理以及实现方法,对游戏代码进行了 ...
C语言程序设计(第二版）主编：余贞侠何钰娟课后习题代码题答案
C语言程序设计(第二版) 主编:余贞侠何钰娟 (课后习题代码题答案) ps.由于没有官方答案,博主将自己写的代码分享出来,若有错误之处请多多谅解,转载注明出处! 版权声明:本文为CSDN博主「Ra ...
c语言程序设计作业word版,c语言程序设计Word版
<c语言程序设计Word版>由会员分享,可在线阅读,更多相关<c语言程序设计Word版(19页珍藏版)>请在人人文库网上搜索. 1.传播优秀Word版文档 ,希望对您有帮助,可 ...
c语言编程将图片上下翻转,C语言实现矩阵翻转(上下翻转、左右翻转)
C语言实现矩阵翻转上下翻转与左右翻转实例代码: #include void matrix (int m, int n, int t) { int arr[m][n]; int i, j, k; f ...
Problem B: C语言习题矩阵元素变换
Problem B: C语言习题矩阵元素变换 Time Limit: 1 Sec Memory Limit: 128 MB Submit: 942 Solved: 558 [Submit][St ...
c语言文件分类二进制,C语言实现文件版（二进制文件版）通讯录
C语言实现文件版(二进制文件版)通讯录 C语言实现文件版(二进制文件版)通讯录通讯录功能添加,删除,查找,修改, 全部, 储存文章目录通讯录功能文件结构一.主函数文件(入口) 二.函数声明文 ...
坦克大战-C语言-详注版
代码地址如下: http://www.demodashi.com/demo/14259.html 坦克大战-C语言-详注版概述本文详述了C语言版坦克大战游戏的原理以及实现方法,对游戏代码进行了详细 ...
个人通讯管理程序C语言,个人通讯录管理系统C语言源程序(优秀版)[1]
个人通讯录管理系统C语言源程序(优秀版)[1] 更新时间:2017/2/22 1:03:00 浏览量:613 手机版 C语言个人通讯录系统源程序: #include /*头文件*/ #includ ...
课程管理系统c语言程序,课程信息管理系统C语言程序Word版
<课程信息管理系统C语言程序Word版>由会员分享,可在线阅读,更多相关<课程信息管理系统C语言程序Word版(19页珍藏版)>请在人人文库网上搜索. 1.传播优秀Word版文 ...

卡尔曼滤波C语言实现（矩阵版）