https://www.bilibili.com/video/BV1ez4y1X7eR
DR.CAN讲的真的很好
卡尔曼滤波的步骤
步骤 | 说明 |
---|---|
Step 1 | 计算卡尔曼增益 |
Step 2 | 更新本次迭代的估计值 |
Step 3 | 更新本次迭代的估计误差 |
具体请看上面DR.CAN的视频
代码
参数 | 说明 |
---|---|
x_mea | 测量值 |
x_est | 估计值 |
e_mea | 固有的测量误差,取决于测量工具的精度,假设测量工具量程是2000/%2,测量误差就是2000*2%=40 |
e_est | 估计误差,每次进行估计后需要更新 |
Kk | 卡尔曼增益 |
大致过程:设定初始值启动卡尔曼滤波,启动完成后开始迭代,代码中初始化与第一个迭代就是卡尔曼滤波的启动过程
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#define Kk_calc(x,y) (x)/(x+y)
struct KalmanFilter {
float x_mea