卡尔曼滤波,C语言,二维

velocity.
*/
float kalman2_filter(kalman2_state *state, float angle,float gyro)
{
float temp0;
float temp1;
float temp;

/* Step1: Predict */
state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1]; //预测当前角速度状态值 自变化+互变化 由于state->A[0][1]为0.1，所以角度的变化是由角速度的测量*0.1得到角度，现有的角度，此式子即是角速度积分累加成角度
state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];//预测当前角度状态值 自变化+互变化 由于state->A[1][0]为0，所以角速度的变化是由角速度的测量自己决定
/* p(n|n-1)=A^2*p(n-1|n-1)+q */
state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0]; //计算角度方差P，实际上是P(k|k-1)=A*P(k-1|k-1) +Q
state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1]; //
state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0]; //
state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1]; //计算角速度方差P，实际上是P(k|k-1)=A*P(k-1|k-1) +Q

/* Step2: Measurement */
/* gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. */
temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1]; // 角度中间变量p * H^T
temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1]; // 角速度中间变量p * H^T
temp = state->r + state->H[0] * temp0 + state->H[1] * temp1; //卡尔曼增益计算公式的分母，测量噪声在此融合r + H * p * H^T]^(-1)，因为角度是计算出来的，角速度是测量出来的，唯一测量噪声只有一个
state->gain[0] = temp0 / temp; //计算角度的卡尔曼增益
state->gain[1] = temp1 / temp; //计算角速度的卡尔曼增益
/* x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/
temp = state->H[0] * state->x[0] + state->H[1] * state->x[1]; //更新当前状态，卡尔曼滤波器也在此输入
state->x[0] = state->x[0] + state->gain[0] *(angle - temp);
state->x[1] = state->x[1] + state->gain[1] * (gyro -temp);

/* Update @p: p(n|n) = [I - gain * H] * p(n|n-1) */
state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0]; //更新协方差
state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];
state->p[1][0] = (1 - state->gain[1] * state->H[0]
) * state->p[1][0];
state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];

return state->x[0]; //卡尔曼输出