卡尔曼滤波算法c语言stm32,一文看懂mpu6050卡尔曼滤波程序-全文

卡尔曼滤波算法c语⾔stm32,⼀⽂看懂mpu6050卡尔曼滤波程

序-全⽂

什么是卡尔曼滤波

卡尔曼滤波(Kalmanfiltering)⼀种利⽤线性系统状态⽅程，通过系统输⼊输出观测数据，对系统状态进⾏最优估计的算法。由于观测数据中包括系统中的噪声和⼲扰的影响，所以最优估计也可看作是滤波过程。

传统的滤波⽅法，只能是在有⽤信号与噪声具有不同频带的条件下才能实现．20世纪40年代，N．维纳和A．H．柯尔莫哥罗夫把信号和噪声的统计性质引进了滤波理论，在假设信号和噪声都是平稳过程的条件下，利⽤最优化⽅法对信号真值进⾏估计，达到滤波⽬的，从⽽在概念上与传统的滤波⽅法联系起来，被称为维纳滤波。这种⽅法要求信号和噪声都必须是以平稳过程为条件。60年代初，卡尔曼(R．E．Kalman)和布塞(R．S．Bucy)发表了⼀篇重要的论⽂《线性滤波和预测理论的新成果》，提出了⼀种新的线性滤波和预测理由论，被称之为卡尔曼滤波。特点是在线性状态空间表⽰的基础上对有噪声的输⼊和观测信号进⾏处理，求取系统状态或真实信号。

这种理论是在时间域上来表述的，基本的概念是：在线性系统的状态空间表⽰基础上，从输出和输⼊观

测数据求系统状态的最优估计。这⾥所说的系统状态，是总结系统所有过去的输⼊和扰动对系统的作⽤的最⼩参数的集合，知道了系统的状态就能够与未来的输⼊与系统的扰动⼀起确定系统的整个⾏为。

卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差(即噪声)，只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进⾏处理，就能在平均的意义上，求得误差为最⼩的真实信号的估计值。因此，⾃从卡尔曼滤波理论问世以来，在通信系统、电⼒系统、航空航天、环境污染控制、⼯业控制、雷达信号处理等许多部门都得到了应⽤，取得了许多成功应⽤的成果。例如在图像处理⽅⾯，应⽤卡尔曼滤波对由于某些噪声影响⽽造成模糊的图像进⾏复原。在对噪声作了某些统计性质的假定后，就可以⽤卡尔曼的算法以递推的⽅式从模糊图像中得到均⽅差最⼩的真实图像，使模糊的图像得到复原。

卡尔曼滤波的性质

①卡尔曼滤波是⼀个算法，它适⽤于线性、离散和有限维系统。每⼀个有外部变量的⾃回归移动平均系统(ARMAX)或可⽤有理传递函数表⽰的系统都可以转换成⽤状态空间表⽰的系统，从⽽能⽤卡尔曼滤波进⾏计算。

②任何⼀组观测数据都⽆助于消除x(t)的确定性。增益K(t)也同样地与观测数据⽆关。

③当观测数据和状态联合服从⾼斯分布时⽤卡尔曼递归公式计算得到的是⾼斯随机变量的条件均值和条件⽅差，从⽽卡尔曼滤波公式给出了计算状态的条件概率密度的更新过程线性最⼩⽅差估计，也就是最⼩⽅差估计。

卡尔曼滤波的应⽤

⽐如，在雷达中，⼈们感兴趣的是跟踪⽬标，但⽬标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利⽤⽬标的动态信息，设法去掉噪声的影响，得到⼀个关于⽬标位置的好的估计。这个估计可以是对当前⽬标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

mpu6050卡尔曼滤波分析

最近在学习卡尔曼滤波算法，算法⾸先静⽌传感器，先测量100次，求平均值，求出偏差Ax_offsetAz_offsetGz_offset.以后每次测量值都减去这⼀偏差。然后通过加速度测得的Ax，Az通过atant(Ax，Az)计算Accel_x即是Roll，K_Angle是klman以后的Roll，Gyro_y为陀螺仪Y轴加速度，K_Gyro_y为卡尔曼之后的数值，

klman是融合Accel_x和Gyro_y，得到的结果。

下图为串⼝发上来的数据分析。

下图为使⽤⼀个软件得出的Roll为klman以后的⾓度，Pinch为原始⾓度。可以看出klman对震动表现较好。但是效果并不是很明显。

mpu6050卡尔曼滤波输出姿态⾓程序短花针茅

*⽂件名：main.c

*描述：I2CEEPROM(AT24C02)测试，测试信息通过USART1打印在电脑的超级终端。

*实验平台：野⽕STM32开发板

*库版本：ST3.0.0

*作者：wildfireteam

**********************************************************************************/

#include“stm32f10x.h”

#include“I2C_MPU6050.h”

#include“usart1.h”

#include“delay.h”

yy公司#include“filter.h”

#include“math.h”

#include“misc.h”

#include“TIMX.h”

#defineAIN2PBout(15)

#defineAIN1PBout(14)

商业地产运营模式#defineBIN1PBout(13)

#defineBIN2PBout(12)

#defineBITBAND(addr，bitnum)((addr&0xF0000000)+0x2000000+((addr&0xFFFFF)《《5)+(bitnum《《2)) #defineMEM_ADDR(addr)*((volaTIleunsignedlong*)(addr))

#defineBIT_ADDR(addr，bitnum)MEM_ADDR(BITBAND(addr，bitnum)) #defineGPIOA_ODR_Addr(GPIOA_BASE+12)//0x4001080C

#defineGPIOB_ODR_Addr(GPIOB_BASE+12)//0x40010C0C

#defineGPIOC_ODR_Addr(GPIOC_BASE+12)//0x4001100C

#defineGPIOD_ODR_Addr(GPIOD_BASE+12)//0x4001140C

#defineGPIOE_ODR_Addr(GPIOE_BASE+12)//0x4001180C

#defineGPIOF_ODR_Addr(GPIOF_BASE+12)//0x40011A0C

#defineGPIOG_ODR_Addr(GPIOG_BASE+12)//0x40011E0C

//#definePAout(n)BIT_ADDR(GPIOA_ODR_Addr，n)//输出

//#definePAin(n)BIT_ADDR(GPIOA_IDR_Addr，n)//输⼊

#definePBout(n)BIT_ADDR(GPIOB_ODR_Addr，n)//输出

//#definePBin(n)BIT_ADDR(GPIOB_IDR_Addr，n)//输⼊

//#definePCout(n)BIT_ADDR(GPIOC_ODR_Addr，n)//输出

//#definePCin(n)BIT_ADDR(GPIOC_IDR_Addr，n)//输⼊

//#definePDout(n)BIT_ADDR(GPIOD_ODR_Addr，n)//输出

//#definePDin(n)BIT_ADDR(GPIOD_IDR_Addr，n)//输⼊

/#definePEout(n)BIT_ADDR(GPIOE_ODR_Addr，n)//输出

//#definePEin(n)BIT_ADDR(GPIOE_IDR_Addr，n)//输⼊

//#definePFout(n)BIT_ADDR(GPIOF_ODR_Addr，n)//输出

//#definePFin(n)BIT_ADDR(GPIOF_IDR_Addr，n)//输⼊

//#definePGout(n)BIT_ADDR(GPIOG_ODR_Addr，n)//输出

//#definePGin(n)BIT_ADDR(GPIOG_IDR_Addr，n)//输⼊

#definePI3.14159265

#defineZHONGZHI-6

#definePWMATIM1-》CCR1//PA8

#definePWMBTIM1-》CCR4//PA11

floatAngle_Balance，Gyro_Balance，Gyro_Turn;//平衡环控制相关变量floatEncoder_left，Encoder_right;//速度环控制相关变量

熊文丹

intMoto1，Moto2;

*函数名：main

*描述：主函数

*输⼊：⽆

*输出：⽆

*返回：⽆

//中断分组处理函数

voidNVIC_Configuration(void)

{

NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置NVIC中断分组2:2位抢占优先级，2位响应优先级}

intRead_Encoder(u8TIMX);

//位置平衡PID控制

intbalance(floatAngle，floatGyro)

{

floatBias，kp=500，kd=1;

intbalance;

Bias=Angle-ZHONGZHI;//===求出平衡的⾓度中值和机械相关

balance=kp*Bias+Gyro*kd;//===计算平衡控制的电机PWMPD控制kp是P系数kd是D系数

returnbalance;

}

//速度PI控制

intvelocity(intencoder_left，intencoder_right)

{

staticfloatVelocity，Encoder_Least，Encoder，Movement;

staticfloatEncoder_Integral，Target_Velocity;

floatkp=50，ki=kp/200;

Encoder_Least=(Encoder_left+Encoder_right)-0;

Encoder*=0.7;//⼀阶低通滤波，上次速度差占70，本次速度差占30，减缓速度差值，减少对直⽴系统的⼲扰Encoder+=Encoder_Least*0.3;//⼀阶低通滤波

机械臂Encoder_Integral+=Encoder;//积分出位移，积分时间10ms

Encoder_Integral=Encoder_Integral-Movement;//接受遥控器的数据，控制正反转

if(Encoder_Integral》15000){

Encoder_Integral=15000;//积分限幅

}

if(Encoder_Integral《-15000)

{

针灸院盗撮Encoder_Integral=-15000;

本文发布于:2024-09-22 10:37:15，感谢您对本站的认可！

本文链接：https://www.17tex.com/xueshu/477299.html

上一篇：ISP模块之RAWDATA去噪（二）--BM3D算法

下一篇：MATLAB的自适应滤波器设计

标签：系统噪声状态滤波观测信号数据

留言与评论（共有 0 条评论）