单片机卡尔曼滤波

卡尔曼滤波原理

品牌型号：Redmibook Pro 15

系统：Windows 10

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。

卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。

卡尔曼滤波算法是什么？

卡尔曼滤波是一个滤波算法，应用非常广泛，它是一种结合先验经验、测量更新的状态估计算法，卡尔曼滤波器是在估计线性系统状态的过程中，以最小均方误差为目的而推导出的几个递推数学等式。

卡尔曼过程中要用到的概念。即什么是协方差，它有什么含义，以及什么叫最小均方误差估计，什么是多元高斯分布。如果对这些有了了解，可以跳过，直接到下面的分割线。

均方误差：

它是"误差"的平方的期望值（误差就是每个估计值与真实值的差），也就是多个样本的时候，均方误差等于每个样本的误差平方再乘以该样本出现的概率的和。

方差：

方差是描述随机变量的离散程度，是变量离期望值的距离。

注意:

两者概念上稍有差别，当你的样本期望值就是真实值时，两者又完全相同。最小均方误差估计就是指估计参数时要使得估计出来的模型和真实值之间的误差平方期望值最小。

卡尔曼滤波

这里的Pdot是一个中间变量，你只看几个步骤是不可能会懂的，最好要全部一起看，下面是纤细步骤：

这里用到的kalman主要分为5个步骤：

(1) X=A*X+B*angular_speed_m;

(2) P=A*P*A'+Q;

(3) X=X-KG(Z-H*X);

(4) K=P*H'(H*P*H'+R);

(5) (I-KG*H)*P;

对矩阵P的更新只有 P=A*P*A'+Q; P,A,Q都是矩阵

A=1 -TS

0 1

Q=q_acce 0

0 q_gyro

单片机目前肯定是不能计算矩阵啦，说以就自己算矩阵乘法，加法(线性代数)

P[0][0]=(KAL_A[0][0]*P[0][0]+KAL_A[0][1]*P[1][0])*KAL_A[0][0]+(KAL_A[0][0]*P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[0][1]+KAL_Q[0][0];/////////////////////

P[0][1]=(KAL_A[0][0]*P[0][0]+KAL_A[0][1]*P[1][0])*KAL_A[1][0]+(KAL_A[0][0]*P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[1][1]+KAL_Q[0][1];////////////////////

P[1][0]=(KAL_A[1][0]*P[0][0]+KAL_A[1][1]*P[1][0])*KAL_A[0][0]+(KAL_A[1][0]*P[0][1]+KAL_A[1][1]*P[1][1])*KAL_A[0][1]+KAL_Q[1][0];////////////////////

P[1][1]=(KAL_A[1][0]*P[0][0]+KAL_A[1][1]*P[1][0])*KAL_A[1][0]+(KAL_A[1][0]*P[0][1]+KAL_A[1][1]*P[1][1])*KAL_A[1][1]+KAL_Q[1][1];////////////////////

然后把等于1或0的式子直接去掉

P[0][0]=(P[0][0]+KAL_A[0][1]*P[1][0])+(P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[0][1]+q_acce;// a

P[0][1]=P[0][1]+KAL_A[0][1]*P[1][1];// b

P[1][0]=P[1][0]+P[1][1]*KAL_A[0][1];// c

P[1][1]=P[1][1]+q_gyro;// d

下面是你写的

P[0][0] += Pdot[0] * dt;

P[1][1] += Pdot[3] * dt;

Pdot[0] = Q_angle - P[0][1] - P[1][0];

Pdot[3] = Q_gyro;

简化，约分后

P[0][0] +=（Q_angle - P[0][1] - P[1][0]）* dt； 对应上面的a

P[1][1] +=Q_gyro * dt; 对应上面的d

当然我自建的kalman和你看的那个版本还是有点差异，不过我的这个版本是可以用的，你看的哪个版本也可以用，效果都差不多。

我的回答就是这样，记得点赞哦！！！！

C51能不能实现卡尔曼滤波，如果可以能不能给我代码？

卡尔曼滤波只是一个算法,而C51是基于标准C语言扩展而来的,你只要明白卡尔曼滤波的数学表达算法,就能用C语言写出来卡尔曼滤波的程序,所以,C语言完全可以写出来卡尔曼滤波算法,C51自然也就能.

但是,这里有个但是!!!

C51虽然是基于标准C语言扩展的,但是,C51是用在51内核单片机上的,而以51内核为内核组成的单片机,大都硬件架构简单,内存容量小,没有专用的硬件乘法器,而且是8位的,基于以上原因,在实际应用中,51单片机是无法完成卡尔曼滤波的.

1 是没有专用硬件乘法/除法器

2 卡尔曼滤波是一种递归算法,需要极大的内存支持,51一般只有几K内存,不足以支持庞大的

卡尔曼滤波.算法

所以,如果你一定要卡尔曼滤波算法,换个强大的MCU吧

单片机卡尔曼滤波的介绍到此就结束了，感谢您耐心阅读，谢谢。