另外你这么难的问题应该给点分才厚道啊!
H文件:
/*
* KalmanFilter.h
* Non-original
* Author:x2d
* Copyright (c) 2012 China
*
*/
#ifndef KalmanFilter_h
#define KalmanFilter_h
#include
class KalmanFilter
{
public:
KalmanFilter()
/*
卡尔曼融合计算
angle_m:加速度计测量唤液并通过atan2(ax,ay)方法计算得到的角度(弧度值)
gyro_m:陀螺仪测量的念卖角速度值(弧度值仔链逗)
dt:采样时间(s)
outAngle:卡尔曼融合计算出的角度(弧度值)
outAngleDot:卡尔曼融合计算出的角速度(弧度值)
*/
void getValue(double angle_m,double gyro_m,double dt,double &outAngle,double &outAngleDot)
private:
double C_0,Q_angle,Q_gyro,R_angle
double q_bias,angle_err,PCt_0,PCt_1,E,K_0,K_1,t_0,t_1
double angle,angle_dot
double P[2][2]
double Pdot[4]
}
CPP文件:
/*
* KalmanFilter.cpp
* Non-original
* Author:x2d
* Copyright (c) 2012 China
*
*/
#include "KalmanFilter.h"
KalmanFilter::KalmanFilter()
{
C_0 = 1.0f
Q_angle = 0.001f
Q_gyro = 0.003f
R_angle = 0.5f
q_bias = angle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f
angle = angle_dot = 0.0f
P[0][0] = 1.0f
P[0][1] = 0.0f
P[1][0] = 0.0f
P[1][1] = 1.0f
Pdot[0] = 0.0f
Pdot[1] = 0.0f
Pdot[2] = 0.0f
Pdot[3] = 0.0f
}
void KalmanFilter::getValue(double angle_m,double gyro_m,double dt,double &outAngle,double &outAngleDot)
{
/*
Serial.print("angle_m = ")
Serial.print(angle_m)
Serial.print("")
Serial.print("gyro_m = ")
Serial.print(gyro_m)
Serial.print("")
*/
angle+=(gyro_m-q_bias) * dt
angle_err = angle_m - angle
Pdot[0] = Q_angle - P[0][1] - P[1][0]
Pdot[1] = -P[1][1]
Pdot[2] = -P[1][1]
Pdot[3] = Q_gyro
P[0][0] += Pdot[0] * dt
P[0][1] += Pdot[1] * dt
P[1][0] += Pdot[2] * dt
P[1][1] += Pdot[3] * dt
PCt_0 = C_0 * P[0][0]
PCt_1 = C_0 * P[1][0]
E = R_angle + C_0 * PCt_0
K_0 = PCt_0 / E
K_1 = PCt_1 / E
t_0 = PCt_0
t_1 = C_0 * P[0][1]
P[0][0] -= K_0 * t_0
P[0][1] -= K_0 * t_1
P[1][0] -= K_1 * t_0
P[1][1] -= K_1 * t_1
angle += K_0 * angle_err
q_bias += K_1 * angle_err
angle_dot = gyro_m-q_bias
outAngle = angle
outAngleDot = angle_dot
/*
Serial.print("angle = ")
Serial.print(angle)
Serial.print("")
Serial.print("angle_dot = ")
Serial.print(angle_dot)
Serial.print("")
*/
}
#endif
欢迎分享,转载请注明来源:内存溢出
评论列表(0条)