扩展卡尔曼滤波 python代码

转载

doscommand 2025-01-04 07:44:44

文章标签 扩展卡尔曼滤波 python代码最优估计卡尔曼滤波状态方程 文章分类 Python 后端开发

卡尔曼滤波学习路线

1. 原理

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

2. 目的

这篇文章主要是自己同样在学习 Kalman filter 的过程中，总结出一套学习的路线，能够让你在了解 Kalman filter 能够有一个清晰的路线

4. 公式

4.1 说明

出现^代表的是估计状态。

扩展卡尔曼滤波 python代码_最优估计

4.1.1 公式一

$扩展卡尔曼滤波 python代码_扩展卡尔曼滤波 python代码_02$

$扩展卡尔曼滤波 python代码_状态方程_03$ ：当前最优估计（即均值，其他地方常用 $扩展卡尔曼滤波 python代码_状态方程_04$ ）
$扩展卡尔曼滤波 python代码_状态方程_05$ ：系数
$扩展卡尔曼滤波 python代码_状态方程_06$ ：前一刻最优估计
$扩展卡尔曼滤波 python代码_卡尔曼滤波_07$ ：系数
$扩展卡尔曼滤波 python代码_状态方程_08$ ：已知外部控制量

4.1.2 公式二

$扩展卡尔曼滤波 python代码_卡尔曼滤波_09$

$扩展卡尔曼滤波 python代码_最优估计_10$ ：当前不确定性（估计误差协方差）
$扩展卡尔曼滤波 python代码_状态方程_05$ ：系数
$扩展卡尔曼滤波 python代码_最优估计_12$ ：前一刻不确定性
$扩展卡尔曼滤波 python代码_状态方程_13$ ：系数
$扩展卡尔曼滤波 python代码_卡尔曼滤波_14$ ：外部环境干扰

总公式

$扩展卡尔曼滤波 python代码_卡尔曼滤波_15$

由上式可知：

当前的最优估计（ $扩展卡尔曼滤波 python代码_状态方程_16$ ）是根据前一刻最优估计（ $扩展卡尔曼滤波 python代码_最优估计_17$ ）预测得到的，并且加上已知外部控制量（ $扩展卡尔曼滤波 python代码_扩展卡尔曼滤波 python代码_18$ ）的修正。
而当前不确定性（ $扩展卡尔曼滤波 python代码_卡尔曼滤波_19$ ）由前一刻不确定性（ $扩展卡尔曼滤波 python代码_状态方程_20$ ）预测得到的，并加上外部环境干扰（ $扩展卡尔曼滤波 python代码_扩展卡尔曼滤波 python代码_21$ ）。

至此，我们对系统可能的动向有了一个模糊的估计，用 $扩展卡尔曼滤波 python代码_最优估计_22$ 和 $扩展卡尔曼滤波 python代码_状态方程_23$

4.1.3 公式三

如果再结合传感器的数据会怎样呢？
$扩展卡尔曼滤波 python代码_卡尔曼滤波_24$

预测值：粉色

传感器测量值：绿色

$扩展卡尔曼滤波 python代码_状态方程_25$ ：最新最优估计
$扩展卡尔曼滤波 python代码_卡尔曼滤波_26$ ：当前最优估计（即均值，其他地方常用 $扩展卡尔曼滤波 python代码_状态方程_04$ ）
$扩展卡尔曼滤波 python代码_状态方程_28$ ：卡尔曼控制增益
$扩展卡尔曼滤波 python代码_扩展卡尔曼滤波 python代码_29$ ：分布的均值，读取到的传感器数据
$扩展卡尔曼滤波 python代码_状态方程_30$ ：矩阵（表示传感器数据）

4.1.4 公式四

$扩展卡尔曼滤波 python代码_最优估计_31$

$扩展卡尔曼滤波 python代码_状态方程_32$ ：最新不确定性
$扩展卡尔曼滤波 python代码_卡尔曼滤波_33$ ：当前不确定性
$扩展卡尔曼滤波 python代码_状态方程_34$ ：卡尔曼增益
$扩展卡尔曼滤波 python代码_状态方程_35$ ：矩阵（表示传感器数据）

4.1.5 公式五

$扩展卡尔曼滤波 python代码_扩展卡尔曼滤波 python代码_36$

$扩展卡尔曼滤波 python代码_最优估计_37$ ：卡尔曼增益
$扩展卡尔曼滤波 python代码_扩展卡尔曼滤波 python代码_38$ ：当前不确定性
$扩展卡尔曼滤波 python代码_最优估计_39$ ：
$扩展卡尔曼滤波 python代码_状态方程_40$ ：
$扩展卡尔曼滤波 python代码_卡尔曼滤波_41$ ：不确定性用协方差（表示传感器噪声）

总公式

$扩展卡尔曼滤波 python代码_状态方程_42$

上式给出了完整的更新步骤方程。

扩展卡尔曼滤波 python代码_扩展卡尔曼滤波 python代码_43

就是新的最优估计，我们可以将它和

扩展卡尔曼滤波 python代码_最优估计_44

放到下一个预测和更新方程中不断迭代。

调整 $扩展卡尔曼滤波 python代码_状态方程_45$ 使得状态误差协方差（ $扩展卡尔曼滤波 python代码_卡尔曼滤波_46$ ）最小。（也就是使测量值和估算的误差最小化）

5. 框图

扩展卡尔曼滤波 python代码_最优估计_47

扩展卡尔曼滤波 python代码_状态方程_48

6. 代码

请跳转至，源码MD说明。

/*

举例而言，R固定，Q越大，代表越信任侧量值，Q无穷代表只用测量值；反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测。

*/

typedef struct {
    float x;  /* state */
    float A;  /* x(n)=A*x(n-1)+u(n),u(n)~N(0,q) */
    float H;  /* z(n)=H*x(n)+w(n),w(n)~N(0,r)   */
    float q;  /* process(predict) noise convariance */
    float r;  /* measure noise convariance */
    float p;  /* estimated error convariance */
    float gain;
} kalman1_state;

void kalman1_init(kalman1_state *state, float init_x, float init_p)
{
    state->x = init_x;  // 待测量的初始值，如有中值一般设成中值（如陀螺仪）
    state->p = init_p;  // 后验状态估计值误差的方差的初始值
    state->A = 1;
    state->H = 1;
    state->q = 2e2;     // 预测噪声协方差
    state->r = 5e2;     // 检测误差协方差
}

float kalman1_filter(kalman1_state *state, float z_measure)
{
    /* Predict */
    // 公式请对照总公式，就能够清楚知道意思了
    state->x = state->A * state->x;
    state->p = state->A * state->A * state->p + state->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;
}