卡尔曼滤波(C语言,二维)(2)

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

以下代码角度测量为例，误差维度为2，即角度和角速度
typedef struct{
float x[2]; //初始状态值矩阵，角度和角速度
float p[2][2]; //初始方差矩阵，角度方差和角速度方差
float A[2][2]; //初始状态矩阵系数，自状态变化系数一般A11与A22为1，相关系数A12，A21系统决定
float H[2]; //测量系数矩阵，角度测量和角速度测量
float q[2]; //过程噪声矩阵，角度过程噪声和角速度过程噪声
float r; //测量噪声矩阵，角度测量噪声，它是一维是因为实际应用中只有测量角速度，而角度是通过角速度计算而来的
float gain[2]; //卡尔曼增益，角度增益和角速度增益
}kalman2_state;
/*********************************************************************************************************
* @brief
* Init fields of structure @kalman1_state.
* I make some defaults in this init function:相关系数A12，A21
* A = {{1, 0.1}, {0, 1}};
* H = {1,0};
* and @q,@r are valued after prior tests.
*
* NOTES: Please change A,H,q,r according to your application.
*
* @inputs
* @outputs
* @retval
*/
void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2])
{
state->x[0] = init_x[0]; //初始角度状态值，可为0
state->x[1] = init_x[1]; //初始角速度状态值，可为0
state->p[0][0] = init_p[0][0]; ////初始角度自变化方差，取小值
state->p[0][1] = init_p[0][1]; ////初始角速度影响角度的方差变化，取小值
state->p[1][0] = init_p[1][0]; ////初始角度影响角速度的方差变化，取小值
state->p[1][1] = init_p[1][1]; ////初始角速度自变化方差，取小值
//state->A = {{1, 0.1}, {0, 1}}; //
state->A[0][0] = 1; //角度自状态变化系数为1
state->A[0][1] = 0.1; //角速度自状态变化系数为1
state->A[1][0] = 0; //角速度影响角度状态变化系数为0.1
state->A[1][1] = 1; //角度影响角速度状态变化系数为0
//state->H = {1,0}; //
state->H[0] = 1; //角度的测量系数
state->H[1] = 0; //角速度的测量系数
//state->q = {{10e-6,0}, {0,10e-6}}; /* measure noise convariance */
state->q[0] = 10e-7; //
state->q[1] = 10e-7; //
state->r = 10e-7; /* estimated error convariance */
}

/*********************************************************************************************************
* @brief
* 2 Dimension kalman filter
* @inputs
* state - Klaman filter structure
* z_measure - Measure value
* @outputs
* state->x[0] - Updated state value, Such as angle,velocity
* state->x[1] - Updated state value, Such as diffrence angle, acceleration

* state->p - Updated estimated error convatiance matrix
* @retval
* Return value is equals to state->x[0], so maybe angle or