#define Q_angle 0.001 // 加速度计过程噪声协方差(过程噪声协方差矩阵对角元素之一)
#define Q_gyro 0.003 // 陀螺仪 过程噪声协方差(过程噪声协方差矩阵对角元素之一)
#define R_angle 0.5 // 测量噪声协方差(不确定因素，故取估计值0.3~0.5)
#define dt 0.01 // 卡尔曼采样时间，同建模中的dt
float KF_Angle,KF_Angle_dot;
float angle, angle_dot;
//const float xdata Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01; //dt的取值为kalman滤波器采样时间
float PP[2][2] = {
{ 1, 0 },
{ 0, 1 }
};
float Pdot[4] ={0,0,0,0}; //////////////////
const char C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
void Kalman_Filter(float angle_temp,float angle_dot_temp)
{
angle+=(angle_dot_temp-q_bias) * dt; //X(k|k-1)=A X(k-1|k-1)+B U(k) ……… (1)
Pdot[0] = Q_angle - PP[0][1] - PP[1][0];
Pdot[1] =- PP[1][1];
Pdot[2] =- PP[1][1];
Pdot[3] = Q_gyro;
//P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2) //Q过程噪声
PP[0][0] += Pdot[0] * dt; // P(k|k-1)=P(k-1|k-1)+Pdot[0]*dt 根据上一状态的最优协方差P(k-1|k-1)推出当前协方差P(k|k-1)
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt; // PP[1][0]=PP[1][0]-PP[1][1]*dt;
PP[1][1] += Pdot[3] * dt; // PP = PP+Pdot*dt
angle_err = angle_temp - angle; //Z(k)-X(k|k-1) 测量值 - 预测值
PCt_0 = C_0 * PP[0][0]; //P(k|k-1) = P(k|k-1)*C_0 当前协方差 C_0=协方差变换增益
PCt_1 = C_0 * PP[1][0]; //P(k|k-1) = P(k|k-1)*C_0
E = R_angle + C_0 * PCt_0; //H P(k|k-1) H’ + R C_0=H*H'=1 R测量噪声
K_0 = PCt_0 / E; //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4) H=1
K_1 = PCt_1 / E; //
t_0 = PCt_0; //
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5) H=1
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1; // PP = PP-K*t
angle += K_0 * angle_err; //X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3) H=1
q_bias += K_1 * angle_err; //X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3) H=1
angle_dot = angle_dot_temp-q_bias;
}
|
