70 lines
2.6 KiB
Plaintext
70 lines
2.6 KiB
Plaintext
package bot4cj.math
|
|
|
|
/**
|
|
* 一维卡尔曼滤波器
|
|
* 使用时先定义一个kalman指针，然后调用kalmanCreate()创建一个滤波器
|
|
* 每次读取到传感器数据后即可调用KalmanFilter()来对数据进行滤波
|
|
* 使用示例
|
|
* extKalman_t p; //定义一个卡尔曼滤波器结构体
|
|
* float SersorData; //需要进行滤波的数据
|
|
* KalmanCreate(&p,20,200); //初始化该滤波器的Q=20 R=200参数
|
|
* while(1)
|
|
* {
|
|
* SersorData = sersor(); //获取数据
|
|
* SersorData = KalmanFilter(&p,SersorData); //对数据进行滤波
|
|
* }
|
|
*/
|
|
@C
|
|
public struct extKalman {
|
|
var X_last: Float32 // 上一时刻的最优结果 X(k-|k-1)
|
|
var P_last: Float32 // 上一时刻最优结果的协方差 P(k-1|k-1)
|
|
var A: Float32 // 系统参数
|
|
var B: Float32
|
|
var Q: Float32
|
|
var R: Float32
|
|
var H: Float32
|
|
|
|
/**
|
|
* @name kalmanCreate
|
|
* @brief 创建一个卡尔曼滤波器
|
|
* @param p: 滤波器
|
|
* T_Q:系统噪声协方差
|
|
* T_R:测量噪声协方差
|
|
*
|
|
* @retval none
|
|
* @attention R固定，Q越大，代表越信任侧量值，Q无穷代表只用测量值
|
|
* 反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测
|
|
*/
|
|
public init(T_Q: Float32, T_R: Float32) {
|
|
X_last = 0.0f32
|
|
P_last = 0.0f32
|
|
Q = T_Q
|
|
R = T_R
|
|
A = 1.0f32
|
|
B = 0.0f32
|
|
H = 1.0f32
|
|
}
|
|
|
|
/**
|
|
* @name KalmanFilter
|
|
* @brief 卡尔曼滤波器
|
|
* @param p: 滤波器
|
|
* dat:待滤波数据
|
|
* @retval 滤波后的数据
|
|
* @attention Z(k)是系统输入,即测量值 X(k|k)是卡尔曼滤波后的值,即最终输出
|
|
* A=1 B=0 H=1 I=1 W(K) V(k)是高斯白噪声,叠加在测量值上了,可以不用管
|
|
* 以下是卡尔曼的5个核心公式
|
|
* 一阶H'即为它本身,否则为转置矩阵
|
|
*/
|
|
public mut func filter(dat: Float32): Float32 {
|
|
let X_mid = A * X_last //百度对应公式(1) x(k|k-1) = A*X(k-1|k-1)+B*U(k)+W(K)
|
|
let P_mid = A * P_last + Q //百度对应公式(2) p(k|k-1) = A*p(k-1|k-1)*A'+Q
|
|
let kg = P_mid / (P_mid + R) //百度对应公式(4) kg(k) = p(k|k-1)*H'/(H*p(k|k-1)*H'+R)
|
|
let X_now = X_mid + kg * (dat - X_mid) //百度对应公式(3) x(k|k) = X(k|k-1)+kg(k)*(Z(k)-H*X(k|k-1))
|
|
let P_now = (1.0f32 - kg) * P_mid //百度对应公式(5) p(k|k) = (I-kg(k)*H)*P(k|k-1)
|
|
P_last = P_now
|
|
X_last = X_now
|
|
return X_now //输出预测结果x(k|k)
|
|
}
|
|
}
|