卡尔曼滤波(2)
来源:互联网 发布:苹果笔记本办公软件 编辑:程序博客网 时间:2024/04/29 08:58
一、卡尔曼算法理解
其实如果不去考虑kalman算法是怎么来的,我们只需要知道有下面几个式子就可以了,具体意思可以看wikipedia
二 卡尔曼滤波算法的实现
这里我的算法是运行在avr单片机上的,所以采用的是c语言写的。下面的代码是要放到avr的定时器中断测试刷新的。用示波器测试了一下,这个算法在16M晶振下的运行时间需要0.35ms,而数据采集需要3ms左右,所以选定定时器时间为8ms.之前也写过一阶的kalman算法,运用在自平衡车上,这边是三阶的,主要是矩阵运算.
//kalman.cfloat dtTimer = 0.008;float xk[9] = {0,0,0,0,0,0,0,0,0};float pk[9] = {1,0,0,0,1,0,0,0,0};float I[9] = {1,0,0,0,1,0,0,0,1};float R[9] = {0.5,0,0,0,0.5,0,0,0,0.01};float Q[9] = {0.005,0,0,0,0.005,0,0,0,0.001};void matrix_add(float* mata,float* matb,float* matc){ uint8_t i,j; for (i=0; i<3; i++){ for (j=0; j<3; j++){ matc[i*3+j] = mata[i*3+j] + matb[i*3+j]; } }}void matrix_sub(float* mata,float* matb,float* matc){ uint8_t i,j; for (i=0; i<3; i++){ for (j=0; j<3; j++){ matc[i*3+j] = mata[i*3+j] - matb[i*3+j]; } }}void matrix_multi(float* mata,float* matb,float* matc){ uint8_t i,j,m; for (i=0; i<3; i++) { for (j=0; j<3; j++) { matc[i*3+j]=0.0; for (m=0; m<3; m++) { matc[i*3+j] += mata[i*3+m] * matb[i*3+j]; } } }}void KalmanFilter(float* am_angle_mat,float* gyro_angle_mat){uint8_t i,j;float yk[9];float pk_new[9];float K[9];float KxYk[9];float I_K[9];float S[9];float S_invert[9];float sdet;//xk = xk + ukmatrix_add(xk,gyro_angle_mat,xk);//pk = pk + Qmatrix_add(pk,Q,pk);//yk = xnew - xkmatrix_sub(am_angle_mat,xk,yk);//S=Pk + Rmatrix_add(pk,R,S);//S求逆invertsdet = S[0] * S[4] * S[8] + S[1] * S[5] * S[6] + S[2] * S[3] * S[7] - S[2] * S[4] * S[6] - S[5] * S[7] * S[0] - S[8] * S[1] * S[3];S_invert[0] = (S[4] * S[8] - S[5] * S[7])/sdet;S_invert[1] = (S[2] * S[7] - S[1] * S[8])/sdet;S_invert[2] = (S[1] * S[7] - S[4] * S[6])/sdet;S_invert[3] = (S[5] * S[6] - S[3] * S[8])/sdet;S_invert[4] = (S[0] * S[8] - S[2] * S[6])/sdet;S_invert[5] = (S[2] * S[3] - S[0] * S[5])/sdet;S_invert[6] = (S[3] * S[7] - S[4] * S[6])/sdet;S_invert[7] = (S[1] * S[6] - S[0] * S[7])/sdet;S_invert[8] = (S[0] * S[4] - S[1] * S[3])/sdet;//K = Pk * S_invertmatrix_multi(pk,S_invert,K);//KxYk = K * Ykmatrix_multi(K,yk,KxYk);//xk = xk + K * ykmatrix_add(xk,KxYk,xk);//pk = (I - K)*(pk)matrix_sub(I,K,I_K);matrix_multi(I_K,pk,pk_new);//update pk//pk = pk_new;for (i=0; i<3; i++){ for (j=0; j<3; j++){ pk[i*3+j] = pk_new[i*3+j]; } }}
三 运用卡尔曼滤波器
这里的kalman滤波器是离散数字滤波器,需要迭代运算。这里把算法放到avr的定时器中断里面执行,进行递归运算.
//isr.c#include "kalman.h"float mpu_9dof_values[9]={0};float am_angle[3];float gyro_angle[3];float am_angle_mat[9]={0,0,0,0,0,0,0,0,0};float gyro_angle_mat[9]={0,0,0,0,0,0,0,0,0};//8MSISR(TIMER0_OVF_vect){//设置计数开始的初始值TCNT0 = 130 ; //8mssei();//采集九轴数据//mpu_9dof_values 单位为g与度/s//加速度计和陀螺仪mpu_getValue6(&mpu_9dof_values[0],&mpu_9dof_values[1],&mpu_9dof_values[2],&mpu_9dof_values[3],&mpu_hmc_values[4],&mpu_hmc_values[5]);//磁场传感器compass_mgetValues(&mpu_9dof_values[6],&mpu_9dof_values[7],&mpu_9dof_values[8]);accel_compass2angle(&mpu_9dof_values[0],&mpu_9dof_values[6],am_angle);gyro2angle(&mpu_9dof_values[3],gyro_angle);am_angle_mat[0] = am_angle[0];am_angle_mat[4] = am_angle[1];am_angle_mat[8] = am_angle[2];gyro_angle_mat[0] = gyro_angle[1];gyro_angle_mat[4] = - gyro_angle[0];gyro_angle_mat[8] = - gyro_angle[2];//卡尔曼KalmanFilter(am_angle_mat,gyro_angle_mat);//输出三轴角度//xk[0] xk[4] xk[8]}
阅读全文
0 0
- 卡尔曼滤波(2)
- 卡尔曼滤波2
- mpu6050姿态解算与卡尔曼滤波(2)卡尔曼滤波
- 卡尔曼滤波(程序)
- 卡尔曼滤波(一)
- 扩展卡尔曼滤波+卡尔曼滤波
- 学习卡尔曼滤波推导笔记系列(2)
- 卡尔曼滤波之Opencv(二)
- 扩展卡尔曼滤波(EKF)
- 也谈卡尔曼滤波(上)
- 卡尔曼滤波(Kalman Filter)
- 卡尔曼滤波_2(图解)
- 卡尔曼滤波学习笔记(1)
- 卡尔曼滤波理解(转载)
- 【SLAM】卡尔曼滤波(Kalman Filter)
- 扩展卡尔曼滤波(EKF)仿真
- 卡尔曼滤波_2(图解)
- 卡尔曼滤波(Kalman Filter)
- 初识WebMagic
- MACfanhonggongjishixian
- ubuntu16.04 打开 关闭 图形界面启动
- 卡尔曼滤波
- SQLiteDatabase 基本使用
- 卡尔曼滤波(2)
- shell基础知识梳理二
- 欢迎使用CSDN-markdown编辑器
- C# 之 TcpClient和Socket
- 股票行情图的绘制,分时图和闪电图
- 以数据之力深化产业格局 中译语通年度峰会将至
- 腾讯推出大数据品牌“腾讯慧聚”,看大数据如何助力政企工作
- 福特取得智能手机壳专利:望取代汽车钥匙
- 出资 23 亿成立合资公司,顺丰建亚洲首个专业货运机场