6-24 2,304PVs
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 |
float kalman1_filter(kalman1_state *state, float z_measure) { /* Predict 预测 状态方程: X(k)=A*X(k-1)+B*U(k)+W(k) 测量方程: Z(k)=H*X(k)+V(k) 预测下一状态: x(k/k-1)=A*X(k-1|k-1)+BU(k),这里U(k)为0。 (1) p表示covariances,误差协方差,当卡尔曼滤波稳定后,应该在一定范围内 P(k|k-1)=A*P(k-1|k-1)A' + Q (2) 通过(1)式可以预测出现在的系统状态,是估计值。然后接下来需要收集现在状态的 测量值,结合预测值和测量值,可以得到现在状态k的最优化估算值x(k|k): x(k|k) = A*x(k|k-1)+Kg(k)(Z(k)-H*A*x(k|k-1)) (3) Kg(k)为卡尔曼增益(Kalman Gain): Kg(k) = P(k|k-1)H'/(H*P(k|k-1)H' + R) (4) 我们的得到了k状态下的最优估计值x(k|k)。但是要让卡尔曼滤波器不断运行, 我们还要更新k状态下x(k|k)的covariance: P(k|k)=(I-Kg(k)*H)*P(k|k-1) (5) */ state->x = state->A * state->x; state->p = state->A * state->A * state->p + state->q; /* p(n|n-1)=A^2*p(n-1|n-1)+q */ /* Measurement */ state->gain = state->p * state->H / (state->p * state->H * state->H + state->r); state->x = state->x + state->gain * (z_measure - state->H * state->x); state->p = (1 - state->gain * state->H) * state->p; return state->x; } |
你没有看错,这么牛的算法,用C语言只有几行。
雁过留声,人过留评
欢迎来访!