基于卡尔曼滤波的气压计高度解算

ax,ay,az为归一化的加速度数据，1代表重力加速度，gx,gy,gz,为加速度数据（单位rad/s）,altitude为气压计测量得到的海拔数据。

//欧拉角
float pitch,roll,yaw;

//世界坐标系下机体加速度
float ax_w,ay_w,az_w;

//世界坐标系下机体速度
float vx_w,vy_w,vz_w;

//世界坐标系下机体位置
float sx_w,sy_w,sz_w;

    
static float dt=0.0025;//采样周期2.5ms 

static float kf_pitch(float X_P,float X_V)//俯仰角一维卡尔曼融合器，X_P为预测量、X_V观测量
{
    static float  P0_0=0,Kg=0.0,P1_0=0;
    float X;
    P1_0=P0_0+0.001f;//系统协方差
    Kg=P1_0/(P1_0+10.0f);//观测协方差
    X=X_P+(X_V-X_P)*Kg;
    P0_0=(1-Kg)*P1_0;
    return X;
}
static float kf_roll(float X_P,float X_V)//横滚角一维卡尔曼融合器，X_P为预测量、X_V观测量
{
    static float  P0_0=0,Kg=0.0,P1_0=0;
    float X;
    P1_0=P0_0+0.001f;//系统协方差
    Kg=P1_0/(P1_0+10.0f);//观测协方差
    X=X_P+(X_V-X_P)*Kg;
    P0_0=(1-Kg)*P1_0;
    return X;
}

static float kf_vz(float X_P,float X_V)//竖直方向速度一维卡尔曼融合器，X_P为预测量、X_V观测量
{
    static float  P0_0=0,Kg=0.0,P1_0=0;
    float X;
    P1_0=P0_0+0.001f;//系统协方差
    Kg=P1_0/(P1_0+1500.0f);//观测协方差
    X=X_P+(X_V-X_P)*Kg;
    P0_0=(1-Kg)*P1_0;
    return X;
}

static float kf_sz(float X_P,float X_V)//竖直方向位置一维卡尔曼融合器，X_P为预测量、X_V观测量
{
    static float  P0_0=0,Kg=0.0,P1_0=0;
    float X;
    P1_0=P0_0+0.001f;//系统协方差
    Kg=P1_0/(P1_0+1500.0f);//观测协方差
    X=X_P+(X_V-X_P)*Kg;
    P0_0=(1-Kg)*P1_0;
    return X;
}

//IMU任务
void imu_task()
{
    //初始角度计算
    pitch=asinf( ay_lpf1/4096.0f);
    roll =asinf(-ax_lpf1/4096.0f);
    yaw  =0;
    
    //初始位移速度计算
    sz_w=altitude_old=altitude;
    vz_w=0;
    
    //imu更新
    while(1)
    {    
        //姿态角度解算（角速度积分同加速度直接解算融合）
        pitch =kf_pitch(pitch+dt*gx,asinf( ay));
        roll  =kf_roll (roll +dt*gy,asinf(-ax));
        yaw  +=dt*gz;

        //世界坐标系下机体加速度解算（根具旋转矩阵计算得到）
        az_w=(-ax*sinf(roll )+ay*cosf(roll )*sinf(pitch)+az*cosf(pitch)*cosf(roll )-az_offset)/4096.0f*9.8f;
        

        //世界坐标下机体速度解算（加速度积分同气压高度差分融合）
        vz_w=kf_vz(vz_w+az_w*dt,(altitude-altitude_old)/dt);altitude_old=altitude;

        //世界坐标下机体位移解算（速度积分同气压数据融合）
        sz_w=kf_sz(sz_w+vz_w*dt,altitude);

        vTaskDelay(configTICK_RATE_HZ/400);//2.5ms调度
    }
}