关于小车站立

有-木-友

学习~~~

灰色头像1

qinlu123 发表于 2012-3-2 16:56
卡尔曼

那应该可以立稳的，可能参数没调好吧。

qinlu123

灰色头像1 发表于 2012-3-3 10:05
那应该可以立稳的，可能参数没调好吧。

你的站稳了吗

yssdsz

qinlu123 发表于 2012-3-2 16:56
卡尔曼

你用的是这段程序吗？？



//加速度计和光电码盘融合求速度

#define FS 100

#define VAR_WA 1

#define VAR_WV 0.04

float T = FS/1;

float A[2][2] = {{1,-T},{0,1}};

float B[2] = {T,0};

float H[2] = {1,0};

float Q[2][2] = {{VAR_WA*T*T,0},{0,0}};

float R = VAR_WV;

float I[2][2] = {{1,0},{0,1}};

float x[2] = {0,0};

float p[2][2] = {{1,0},{0,1}};

float tmp_x[2];

float tmp_p[2][2];

float kg[2];

void kalman(float ai,float vi){

        tmp_x[0] = A[0][0]*x[0]+A[0][1]*x[1]+B[0]*ai;

        tmp_x[1] = A[1][0]*x[0]+A[1][1]*x[1]+B[1]*ai;

        tmp_p[0][0] = (A[0][0]*p[0][0]+A[0][1]*p[1][0])*A[0][0]+(A[0][0]*p[0][1]+A[0][1]*p[1][1])*A[0][1]+Q[0][0];

        tmp_p[0][1] = (A[0][0]*p[0][0]+A[0][1]*p[1][0])*A[1][0]+(A[0][0]*p[0][1]+A[0][1]*p[1][1])*A[1][1]+Q[0][1];

        tmp_p[1][0] = (A[1][0]*p[0][0]+A[1][1]*p[1][0])*A[0][0]+(A[1][0]*p[0][1]+A[1][1]*p[1][1])*A[0][1]+Q[1][0];

        tmp_p[1][1] = (A[1][0]*p[0][0]+A[1][1]*p[1][0])*A[1][0]+(A[1][0]*p[0][1]+A[1][1]*p[1][1])*A[1][1]+Q[1][1];

        kg[0] = (tmp_p[0][0]*H[0]+tmp_p[0][1]*H[1])/((H[0]*tmp_p[0][0]+H[1]*tmp_[1][0])*H[0]+(H[0]*tmp_p[0][1]+H[1]*tmp_[1][1])*H[1]+R);

        kg[1] = (tmp_p[1][0]*H[0]+tmp_p[1][1]*H[1])/((H[0]*tmp_p[0][0]+H[1]*tmp_[1][0])*H[0]+(H[0]*tmp_p[0][1]+H[1]*tmp_[1][1])*H[1]+R);

        x[0] = tmp_x[0]+kg[0]*(vi-H[0]*tmp_x[0]+H[1]*tmp_x[1]);

        x[1] = tmp_x[1]+kg[1]*(vi-H[0]*tmp_x[0]+H[1]*tmp_x[1]);

        p[0][0] = (I[0][0]-kg[0]*H[0])*tmp_p[0][0];

        p[0][1] = (I[0][1]-kg[0]*H[1])*tmp_p[0][1];

        p[1][0] = (I[1][0]-kg[1]*H[0])*tmp_p[1][0];

        p[1][1] = (I[1][1]-kg[1]*H[1])*tmp_p[1][1];

}

erchowyo

erchowyo

yssdsz

这个，对吧？


//*
//-------------------------------------------------------
//Kalman滤波，8MHz的处理时间约1.8ms；
//-------------------------------------------------------
float angle, angle_dot;   //外部需要引用的变量
//-------------------------------------------------------
float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.006;
   //注意：dt的取值为kalman滤波器采样时间;
float P[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_m,float gyro_m)   //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias) * dt;//先验估计

Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;

P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;


angle_err = angle_m - angle;//zk-先验估计


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;//Kk
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;//输出值（后验估计）的微分 = 角速度
}

qinlu123

yssdsz 发表于 2012-3-3 11:31
你用的是这段程序吗？？

不是啊

qinlu123

yssdsz 发表于 2012-3-3 12:29
这个，对吧？

也不是

yssdsz

qinlu123 发表于 2012-3-3 12:40
也不是

这个？？





/***************************************************************************/
/* kalman.c                                                                */
/* 1-D 卡尔曼滤波算法, 通过加速度计（倾角）和陀螺仪完成                    */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.0                                                            */
/* Date:30.05.2003                                                         */
/* 改编自 Trammel Hudson(hudson@rotomotion.com)                            */  
/* -------------------------------                                         */
/* 程序编译:                                                               */
/*      Linux                                                              */
/*      gcc68 -c  XXXXXX.c (to create object file)                         */
/*      gcc68 -o  XXXXXX.hex XXXXXX.o ppwa.o                               */
/* Upload data :               */
/* ul filename.txt                               */
/***************************************************************************/
/* 本版内容:                                                               */
/***************************************************************************/
/* This is a free software; you can redistribute it and/or modify          */
/* it under the terms of the GNU General Public License as published       */
/* by the Free Software Foundation; either version 2 of the License,       */
/* or (at your option) any later version.                                  */
/*                                                                         */
/* this code is distributed in the hope that it will be useful,            */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of          */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           */
/* GNU General Public License for more details.                            */
/*                                                                         */
/* You should have received a copy of the GNU General Public License       */
/* along with Autopilot; if not, write to the Free Software                */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston,                   */
/* MA  02111-1307  USA                                                     */
/***************************************************************************/
#include <math.h>
#include "eyebot.h"
/*
* 陀螺仪采样周期20ms
* 更新频率不同的话可以改变
*/               
static const float dt = 0.02;   //陀螺仪采样周期
/*
* 协方差矩阵.每次更新决定传感器跟踪实际状态的好坏
*/
static float P[2][2] = {{ 1, 0 },   //协方差矩阵
                    { 0, 1 }};
/*
* 两个状态，角度和陀螺仪偏差.作为计算角度和偏差的副产品，我们可以得到无偏的角速率
* 以下是该模块对于作者的只读量
*/
float angle;
float q_bias;
float rate;      //3个无偏输出量

/*
* R为测量协方差noise.R=E[vvT]
* 这种情况下为1×1矩阵
* 0.1 rad jitter from the inclinometer.
* for a 1x1 matrix in this case v = 0.1
*/
static const float R_angle = 0.001 ;

/*
* Q是2×2过程协方差矩阵噪声.
* 表示对加速度计和陀螺仪的信任程度
*/
static const float Q_angle = 0.001;
static const float Q_gyro  = 0.0015;

/*
* 状态每次通过带有偏移量量的陀螺仪测量值进行更新。更新当前的角度和角速率
*
* 采集到的陀螺仪测量值应该转换为实际单位,但是不需要进行偏移补偿。滤波器会自动跟踪偏移量
*
*   A = [ 0 -1 ]
*       [ 0  0 ]
*/
void stateUpdate(const float q_m){
float q;
float Pdot[4];
/*去除陀螺仪偏差*/
q = q_m - q_bias;//当前角速度：测量值-偏差
/*
  * Compute the derivative of the covariance matrix
  * 计算协方差矩阵的导数
  * (equation 22-1)
  * Pdot = A*P + P*A' + Q
  */
Pdot[0] = Q_angle - P[0][1] - P[1][0]; /* 0,0 */
Pdot[1] = - P[1][1];          /* 0,1 */
Pdot[2] = - P[1][1];          /* 1,0 */
Pdot[3] = Q_gyro;   /* 1,1 */
/* 保存陀螺仪角速率无偏估计*/
rate = q;
/*
   * Update our angle estimate
   * angle += angle_dot * dt
   *       += (gyro - gyro_bias) * dt
   *       += q * dt
   */
angle += q * dt;//角速度积分累加到 估计角度
/* 更新协方差矩阵 */
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;       //why？
}

/*
* kalman_update在加速度计更新时由用户调用
* 不需要每次都调用,但加速度计采样频率和陀螺仪一样的话就可以
*
*         H  = [ 1 0 ]
*
* 因为角度测量直接符合角度估计而且角度测量和陀螺仪没有关联
*/
void kalmanUpdate(const float incAngle)
{
/* 估计角度测量值和测量偏差 */
float angle_m = incAngle;
float angle_err = angle_m - angle;//1.12 zk-H*xk_dot
/*
   h_0表示状态估计值和状态量的相关程度
   *      H = [h_0 h_1]
   *
   * The h_1 shows that the state measurement does not relate
   * to the gyro bias estimate.  We don't actually use this, so
   * we comment it out.
   */
float h_0 = 1;
/* const float  h_1 = 0; */
/*
   * 先验 P*H' 用到两次
   */
const float PHt_0 = h_0*P[0][0]; /* + h_1*P[0][1] = 0*/
const float PHt_1 = h_0*P[1][0]; /* + h_1*P[1][1] = 0*/
/*
   * 估计误差:
   * (equation 21-1)
   * E = H P H' + R
   */
float E = R_angle +(h_0 * PHt_0);
/*
   * 估计卡尔曼增益：
   * (equation 21-2)
   *
   * K = P H' inv(E)
          */
float K_0 = PHt_0 / E;
float K_1 = PHt_1 / E;
   
/*
   * 更新协方差矩阵:
   * (equation 21-3)
   *
   * P = P - K H P
          * Let
  *      Y = H P
   */
float Y_0 = PHt_0;  /*h_0 * P[0][0]*/
float Y_1 = h_0 * P[0][1];
   
P[0][0] -= K_0 * Y_0;
P[0][1] -= K_0 * Y_1;
P[1][0] -= K_1 * Y_0;
P[1][1] -= K_1 * Y_1;
/*
   * 后验估计:
   *
   * Xnew = X + K * error
   *
   * err is a measurement of the difference in the measured state
   * and the estimate state.  In our case, it is just the difference
   * between the inclinometer measured angle and the estimated angle.
   */
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
}