通用二阶卡尔曼滤波算法
因为c语言不支持矩阵运算，卡尔曼滤波算法涉及矩阵的运算，所以我写了一个通用性比较强的矩阵运算“库”。 有了它，矩阵的交叉、点乘法、求倒排、求二次矩阵的逆等操作就变得简单了。
函数列表：
void矩阵_ init (float * pr，int hang，int lie )； void矩阵_ init _ one (float *矩阵，int hang，int lie )； void矩阵_ init _ eye (浮动*矩阵，int hang，int lie )； voidmatrix_mul_cross(float*pre，int hang_pre，int lie_pre，float *later，int hang_lat，int lie_lat，flat void矩阵_ t (float * matrix，int hang，int lie，float *res )； void矩阵_ ni (float * matrix _ a，int scale，float *res )； voidmatrix_add(float*matrix_a，float *matrix_b，int hang，int lie，float *res )； void矩阵_ deduct (float * matrix _ a，float *matrix_b，int hang，int lie，float *res )； 基于我自己编写的矩阵运算库的二阶卡尔曼滤波源代码：
公式如下。
/速度位置二次系统void horizon _ klm _ estimate (float pos，float Vel，Second_order_sys * sys，float delta_T ) sys-a _ mm sys-y_oberve[1][0]=Vel； sys-x _ state [0] [0]=sys-x _ state [0] [0] delta _ t * sys-x _ state [1] [0]；/step1predictx//step2predictpfloatp _ temp [2] [2]，P_temp2[2][2]； float A_T_Matrix[2][2]； matrix_mul_cross((float * ) sys-A_matrix )，2，2，) float * (sys-p _ variance )，2，2，2，float * ) matrix_t () float * ) sys-A_matrix )，2，2，) float * (a _ t _ matrix )； matrix_mul_cross((float * ) P_temp，2，2，) float * (a _ t _ matrix，2，2，) float * ) P_temp2)； matrix_add () float * ) P_temp2，) float * ) sys-Q_variance )，2，2，) float * (sys-p _ variance ) )///SSS float K_temp[2][2]； 矩阵_ add (浮动* ) sys-p_variance )、浮动sys-R_variance )、2，2，)浮动K_temp )； 矩阵3 _ ni () float * ) K_temp，2，) ) float * ) matrix3_NI )； matrix _ mul _ cross (//step4correctxfloatz _ delta [2] [1]； float statue_correct[2][1]； z _ delta [0] [0]=sys-x _ state [0] [0]-sys-y _ ober ve [0] [0]； z _ delta [1] [0]=sys-x _ state [1] [0]-sys-y _ ober ve [1] [0]； matrix_mul_cross((float * ) sys-K_gain )，2，2，(float * ) Z_delta，2，1，() float * ) statue _ corr red sys-x _ state [1] [0]=sys-x _ state [1] [0]-statue _ correct [1] [0]； //step 5更新datepfloatp _ update [2] [2]； 矩阵_ init _ eye ((float * ) P_temp，2，2 )； 矩阵_ deduct () float * ) P_temp，) float * ) ) sys-K_gain )，2，) float * ) P_temp )； matrix_mul_cross () float * ) P_temp，2，2，) float * (sys-p _ variance )，2，2，2，) float * (p _ u u u pdand }卡尔曼滤波结构：
typedef struct { floatx _ state [2] [1]； //状态float y_oberve[2][1]； float A_matrix[2][2]； float p_variance[2][2]； //协方差float K_gain[2][2]； //卡尔曼增益float R_variance[2][2]； //观测误差方差float Q_variance[2][2]； //工艺误差方差}Second_order_sys； 如果有伙伴的话，你会发现这里没有h矩阵。 我的这个位置-速度系统，我觉得h矩阵是单位排列，运算时有无都一样。
这里以二维数组为矩阵，左边为行数，右边为列数，[2][1]为2行1列的矩阵。
函数调用：
int Horizon_counter=0； void horizon _ pos _ estimate ({ horizon _ PV.e _ vel=hubu _ filter (0.75，LC306_flow.flow_x，u u u UAV _ GPS how ) horizon _ PV.e _ pos=UAV _ GPS _ home.e _ deviation； horizon _ PV.n _ pos=UAV _ GPS _ home.n _ deviation； Horizon_counter； if(Horizon_counter100 ) {Horizon_counter=0； }if(Horizon_counter%5==0) horizon _ klm _ estimate (UAV _ GPS _ home.e _ deviation，horizon _ PV.e _ e }代码量少，容易理解，所以我自己觉得非常容易使用。
想要变更为其他形式的系统时，可以变更a矩阵和h矩阵。
如果你想使用我这个矩阵运算库，请看我上传的资源：
3359 download.csdn.net/download/weixin _ 39956482/37316224