|
本帖最后由 salahading 于 2014-11-13 20:33 编辑
- /**********Kalman_Filter *********/
- /* 系统状态变量*/
- float x[2] = {0,0};
- /* 采样时间 */
- float Ts = 0.001;
- /*误差的协方差矩阵*/
- float p[2][2] = {{1,0},{0,1}};
- /* Kalman增益 */
- float K[2] = {0,0};
- /* 系统状态协方差矩阵对角元素 */
- float Q0 = 0.00001,Q1 = 0.00003;
- /* 系统输出 高斯噪声方差*/
- float R = 0.5;
- /*
- Zk 倾角仪器或加速度计测量的系统角度 Gyro 陀螺仪的角速度输出
- x[0] 系统倾角
- x[1] 陀螺仪漂移
- */
- void Kalman_Filter(float Zk,float Gyro)
- {
-
- /*
- X(k+1) = AX(k) + B*U
- A = {{1,-Ts},{0,1}} B = {Ts,0}
- Zk = Hx
- Zk 系统输出 H = {1 ,0}
- 计算系统状态估计值
- */
- x[0] = x[0] + Ts*(Gyro - x[1]);
- x[1] = x[1];
- /*
- 误差的协方差估计
- */
- p[0][0] = p[0][0] - Ts*(p[0][1]+p[1][0]) + Ts*Ts*p[1][1] + Q0;
- p[0][1] = p[0][1] - Ts*p[1][1];
- p[1][0] = p[1][0] - Ts*p[1][1];
- p[1][1] = p[1][1] + Q1;
-
- /*
- 计算Kalman增益
- */
- K[0] = p[0][0]/(p[0][0]+R);
- K[1] = p[1][0]/(p[0][0]+R);
-
- /*
- 校正先验协方差估计
- */
- p[0][0] = p[0][0] - K[0]*p[0][0];
- p[0][1] = p[0][1] - K[0]*p[0][1];
- p[1][0] = p[1][0] - K[1]*p[0][0];
- p[1][1] = p[1][1] - K[1]*p[0][1];
-
- /*
- 校正系统状态估计值
- */
- x[0] = x[0] + K[0]*(Zk - x[0]);
- x[1] = x[1] + K[1]*(Zk - x[0]);
-
- }
复制代码
有同学说的他人的程序是不是下面的
- void Kalman_Filter1(float Accel,float Gyro)
- {
- Angle+=(Gyro - Q_bias) * dt; //先验估计
-
- Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
- Pdot[1]=- PP[1][1];
- Pdot[2]=- PP[1][1];
- Pdot[3]=Q_gyro;
-
- PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
- PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
- PP[1][0] += Pdot[2] * dt;
- PP[1][1] += Pdot[3] * dt;
-
- Angle_err = Accel - Angle; //zk-先验估计
-
- PCt_0 = C_0 * PP[0][0];
- PCt_1 = C_0 * PP[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 * PP[0][1];
- PP[0][0] -= K_0 * t_0; //后验估计误差协方差
- PP[0][1] -= K_0 * t_1;
- PP[1][0] -= K_1 * t_0;
- PP[1][1] -= K_1 * t_1;
-
- Angle += K_0 * Angle_err; //后验估计
- Q_bias += K_1 * Angle_err; //后验估计
- Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
- }
复制代码
那这个是不是卡尔曼滤波呢? |
阿莫论坛20周年了!感谢大家的支持与爱护!!
如果想吃一顿饺子,就得从冰箱里取出肉,剁馅儿,倒面粉、揉面、醒面,擀成皮儿,下锅……
一整个繁琐流程,就是为了出锅时那一嘴滚烫流油的热饺子。
如果这个过程,禁不住饿,零食下肚了,饺子出锅时也就不香了……《非诚勿扰3》
|