卡尔曼滤波原理及应用（二）

        在上一篇文章介绍并推导了卡尔曼滤波的基本原理之后，本文主要通过几个实例来讲解卡尔曼滤波的具体应用。

一.基于卡尔曼滤波的数据融合示例

        假设使用一把最小刻度为1mm的直尺去测量一个长度为0.01111m的物体，一般的做法为多次测量后取平均值，即：

        对本公式采用数据融合的方法进行修正：

     则物体本身的状态空间表达式可写为：

        假设初始状态,,，轮趣科技的示例由python编写，本文分别由matlab和C++给出，卡尔曼滤波的五个基本公式如下：

①先验估计：

②后验估计：

代码如下：

% 初始化参数
n = 100; % 迭代次数
x = 0.01111; % 真实值
Q = 1e-10;
R = 1e-3^2;
x_hat = zeros(1, n);
p = zeros(1, n);
x_hat_minus = zeros(1, n);
p_minus = zeros(1, n);
K = zeros(1, n);
y = normrnd(x, 1e-3, [1, n]); % 生成带噪声的测量值

% 初始化
x_hat(1) = 0.0;
p(1) = 1.0;

% 卡尔曼滤波
for k = 2:n
    % 预测部分
    x_hat_minus(k) = x_hat(k - 1);
    p_minus(k) = p(k - 1) + Q;

    % 校正部分
    K(k) = p_minus(k) / (p_minus(k) + R);
    x_hat(k) = x_hat_minus(k) + K(k) * (y(k) - x_hat_minus(k));
    p(k) = (1 - K(k)) * p_minus(k);
end

% 绘制图像
figure;
hold on;
plot(y, 'r+', 'DisplayName', 'noisy measurements');
plot(x_hat, 'g-', 'DisplayName', 'a posteri estimate');
yline(x, 'b', 'DisplayName', 'truth value');
legend;
xlabel('Iteration');
ylabel('Voltage');
hold off;

编写C++代码如下：

#include <iostream>
#include <vector>
#include <random>
#include <cmath>
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;

int main() {
    int n = 100; // 迭代次数
    double x = 0.01234; // 真实值
    double Q = 1e-10;
    double R = std::pow(1e-3, 2);
    std::vector<double> x_hat(n, 0.0);
    std::vector<double> p(n, 0.0);
    std::vector<double> x_hat_minus(n, 0.0);
    std::vector<double> p_minus(n, 0.0);
    std::vector<double> K(n, 0.0);
    std::vector<double> y(n, 0.0);

    // 生成带噪声的测量值
    std::default_random_engine generator;
    std::normal_distribution<double> distribution(x, 1e-3);
    for (int i = 0; i < n; ++i) {
        y[i] = distribution(generator);
    }

    // 初始化
    x_hat[0] = 0.0;
    p[0] = 1.0;

    // 卡尔曼滤波
    for (int k = 1; k < n; ++k) {
        // 预测部分
        x_hat_minus[k] = x_hat[k - 1];
        p_minus[k] = p[k - 1] + Q;

        // 校正部分
        K[k] = p_minus[k] / (p_minus[k] + R);
        x_hat[k] = x_hat_minus[k] + K[k] * (y[k] - x_hat_minus[k]);
        p[k] = (1 - K[k]) * p_minus[k];
    }

    // 绘制图像
    plt::figure_size(700, 400);
    plt::plot(y, "r+");
    plt::plot(x_hat, "g-");
    plt::axhline(x, {{"color", "blue"}});
    plt::xlabel("Iteration");
    plt::ylabel("Voltage");
    plt::legend({"noisy measurements", "a posteri estimate", "truth value"});
    plt::show();

    return 0;
}

编写Cmake文件：

cmake_minimum_required(VERSION 3.10)
project(KalmanFilter)

# 设置 C++ 标准
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED True)

# 查找 Python 解释器和库
find_package(Python3 REQUIRED COMPONENTS Interpreter Development NumPy)

# 查找 matplotlib-cpp
find_path(MATPLOTLIBCPP_INCLUDE_DIRS "matplotlibcpp.h" PATHS /usr/include /usr/local/include)

# 添加头文件目录
include_directories(${MATPLOTLIBCPP_INCLUDE_DIRS})
include_directories(${Python3_INCLUDE_DIRS} ${Python3_NumPy_INCLUDE_DIRS})

# 查找 Python 库
find_library(PYTHON_LIBRARIES Python3::Python)

# 添加可执行文件
add_executable(KalmanFilter main.cpp)

# 链接库
target_link_libraries(KalmanFilter ${PYTHON_LIBRARIES})

运行结果如下：

二.卡尔曼滤波估算车身倾角（MPU6050）

        

        MPU6050 是一款集成了三轴陀螺仪和三轴加速度计的传感器，广泛应用于姿态检测、运动跟踪和稳定控制等领域。具有如下主要特点：

1. 三轴加速度计：可以测量设备在 X、Y、Z 轴上的线性加速度。
2. 三轴陀螺仪：可以测量设备在 X、Y、Z 轴上的角速度。

        选取的状态变量为陀螺仪的倾角、角速度，静差作为作为状态变量：

        则系统的状态空间方程如下：

        控制输入为：

        其中：

        对上述方程采用零阶保持法进行离散化得到：

        

        观测方程：

        则离散状态空间表达式如下：

        带入卡尔曼滤波的五个公式中得到：

 ①先验估计：

②后验估计：

C代码如下：

#include <stdio.h>
#include <math.h>

#define DT 0.01         // 时间步长
#define Q_ANGLE 0.01    // 角度过程噪声方差
#define Q_ANGULAR_VELOCITY 0.01  // 角速度过程噪声方差
#define Q_BIAS 0.001    // 偏差过程噪声方差
#define R_ANGLE 0.1     // 角度测量噪声方差
#define R_ANGULAR_VELOCITY 0.1 // 角速度测量噪声方差

// 状态向量和协方差矩阵
typedef struct {
    double angle;       // 角度
    double angular_vel; // 角速度
    double bias;        // 偏差
} state;

typedef struct {
    double angle;       // 测量角度
    double angular_vel; // 测量角速度
} measurement;

// 卡尔曼滤波参数
state x = {0.0, 0.0, 0.0};   // 初始状态
double P[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};  // 初始协方差矩阵

// 离散化过程矩阵和噪声协方差
double Ad[3][3] = {
    {1, DT, 0},
    {0, 1, 0},
    {0, 0, 1}
};
double Bd[3][1] = {{0}, {DT}, {0}};  // 输入矩阵
double Qd[3][3] = {
    {Q_ANGLE, 0, 0},
    {0, Q_ANGULAR_VELOCITY, 0},
    {0, 0, Q_BIAS}
}; // 过程噪声协方差矩阵

// 测量矩阵和测量噪声协方差矩阵
double H[2][3] = {
    {1, 0, 0},
    {0, 1, 0}
};
double Rm[2][2] = {
    {R_ANGLE, 0},
    {0, R_ANGULAR_VELOCITY}
}; // 测量噪声协方差矩阵

// 矩阵乘法函数
void matrixMultiply(double a[3][3], double b[3][3], double result[3][3]) {
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            result[i][j] = 0;
            for (int k = 0; k < 3; k++) {
                result[i][j] += a[i][k] * b[k][j];
            }
        }
    }
}

// 矩阵转置函数
void matrixTranspose(double a[3][3], double result[3][3]) {
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            result[i][j] = a[j][i];
        }
    }
}

// 卡尔曼滤波函数
void kalman_filter(measurement z, double u) {
    // 预测步骤
    // 预测状态
    double x_pred[3] = {
        Ad[0][0] * x.angle + Ad[0][1] * x.angular_vel + Ad[0][2] * x.bias + Bd[0][0] * u,
        Ad[1][0] * x.angle + Ad[1][1] * x.angular_vel + Ad[1][2] * x.bias + Bd[1][0] * u,
        Ad[2][0] * x.angle + Ad[2][1] * x.angular_vel + Ad[2][2] * x.bias + Bd[2][0] * u
    };

    // 预测协方差
    double P_pred[3][3];
    double Ad_T[3][3];
    matrixTranspose(Ad, Ad_T);
    matrixMultiply(Ad, P, P_pred);
    matrixMultiply(P_pred, Ad_T, P_pred);
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            P_pred[i][j] += Qd[i][j];
        }
    }

    // 更新步骤
    // 计算卡尔曼增益
    double H_T[3][2] = {
        {1, 0},
        {0, 1},
        {0, 0}
    };
    double S[2][2] = {
        {H[0][0] * P_pred[0][0] + Rm[0][0], H[0][1] * P_pred[0][1] + Rm[0][1]},
        {H[1][0] * P_pred[1][0] + Rm[1][0], H[1][1] * P_pred[1][1] + Rm[1][1]}
    };
    double S_inv[2][2] = {
        {1.0 / S[0][0], 0},
        {0, 1.0 / S[1][1]}
    };
    double K[3][2];
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 2; j++) {
            K[i][j] = 0;
            for (int k = 0; k < 2; k++) {
                K[i][j] += P_pred[i][k] * H_T[k][j] * S_inv[j][j];
            }
        }
    }

    // 更新状态
    double y[2] = {z.angle - x_pred[0], z.angular_vel - x_pred[1]};
    for (int i = 0; i < 3; i++) {
        x.angle += K[i][0] * y[0];
        x.angular_vel += K[i][1] * y[1];
        x.bias += K[i][2] * y[2];
    }

    // 更新协方差
    double KH[3][3];
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            KH[i][j] = 0;
            for (int k = 0; k < 2; k++) {
                KH[i][j] += K[i][k] * H[k][j];
            }
        }
    }
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            P[i][j] = (i == j ? 1 : 0) - KH[i][j];
        }
    }
    double temp[3][3];
    matrixMultiply(P, P_pred, temp);
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            P[i][j] = temp[i][j];
        }
    }
}

int main() {
    measurement z = {0.0, 0.0};
    double u = 0.0;

    // 模拟数据并调用卡尔曼滤波器
    for (int i = 0; i < 100; i++) {
        // 模拟传感器数据
        z.angle = sin(i * DT);
        z.angular_vel = cos(i * DT);
        u = z.angular_vel;

        // 调用卡尔曼滤波器
        kalman_filter(z, u);

        // 输出滤波后的状态
        printf("angle: %f, angular_vel: %f, bias: %f\n", x.angle, x.angular_vel, x.bias);
    }

    return 0;
}

三.基于卡尔曼滤波的车辆位姿估计

        选择输入为车辆的横摆角速度与纵向速度，车辆的过程变量为x,y坐标以及车辆航向角：

        

        连续时间系统的状态空间模型为：

        观测方程为：

        采用零阶保持（ZOH）方法离散化，系统矩阵 𝐴𝑑​ 的离散化过程如下：

        

        输入矩阵 𝐵𝑑​ 的离散化：

        则系统离散化后的状态空间模型为：

        套用卡尔曼滤波公式为：

        

  ①先验估计：

②后验估计：

        编写matlab代码如下

function kalman_filter_simulation()
    % 参数定义
    DT = 0.01;         % 时间步长
    Q_X = 0.01;        % X 轴过程噪声方差
    Q_Y = 0.01;        % Y 轴过程噪声方差
    Q_THETA = 0.01;    % 航向角过程噪声方差
    R_X = 0.1;         % X 轴测量噪声方差
    R_Y = 0.1;         % Y 轴测量噪声方差

    % 初始状态和协方差矩阵
    x = [0; 0; 0];  % 初始状态 [x, y, theta]
    P = eye(3);     % 初始协方差矩阵

    % 离散化过程噪声协方差矩阵
    Qd = diag([Q_X, Q_Y, Q_THETA]);

    % 测量噪声协方差矩阵
    Rm = diag([R_X, R_Y]);

    % 模拟数据
    N = 100;
    true_state = zeros(3, N);
    est_state = zeros(3, N);
    measurement_data = zeros(2, N);
    control_input = zeros(2, N);
    for i = 1:N
        true_state(:, i) = [i * DT; 0.5 * i * DT; 0.1 * i * DT]; % 模拟真实轨迹
        measurement_data(:, i) = true_state(1:2, i) + sqrt(Rm) * randn(2, 1); % 加入测量噪声
        control_input(:, i) = [0.5; 0.1]; % 控制输入
    end

    % 卡尔曼滤波过程
    for k = 1:N
        [x, P] = kalman_filter(x, P, measurement_data(:, k), control_input(:, k), DT, Qd, Rm);
        est_state(:, k) = x;
    end

    % 计算误差
    errors = true_state - est_state;

    % 可视化
    figure;
    subplot(3, 1, 1);
    plot(1:N, errors(1, :));
    title('X 轴误差');
    xlabel('时间步长');
    ylabel('误差');

    subplot(3, 1, 2);
    plot(1:N, errors(2, :));
    title('Y 轴误差');
    xlabel('时间步长');
    ylabel('误差');

    subplot(3, 1, 3);
    plot(1:N, errors(3, :));
    title('航向角误差');
    xlabel('时间步长');
    ylabel('误差');
end

function [x, P] = kalman_filter(x, P, z, u, DT, Qd, Rm)
    % 更新矩阵
    v = u(1);
    theta = x(3);
    Ad = [1, 0, -v * sin(theta) * DT;
          0, 1, v * cos(theta) * DT;
          0, 0, 1];
    Bd = [cos(theta) * DT, 0;
          sin(theta) * DT, 0;
          0, DT];
    H = [1, 0, 0;
         0, 1, 0];

    % 预测步骤
    x_pred = Ad * x + Bd * u;
    P_pred = Ad * P * Ad' + Qd;

    % 更新步骤
    K = P_pred * H' / (H * P_pred * H' + Rm);
    x = x_pred + K * (z - H * x_pred);
    P = (eye(3) - K * H) * P_pred;
end

编写C++代码如下：

#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;

using namespace Eigen;
using namespace std;

#define DT 0.1         // 时间步长
#define Q_X 0.01       // X 轴过程噪声方差
#define Q_Y 0.01       // Y 轴过程噪声方差
#define Q_THETA 0.01   // 航向角过程噪声方差
#define R_X 0.1        // X 轴测量噪声方差
#define R_Y 0.1        // Y 轴测量噪声方差

typedef Vector3d State;
typedef Vector2d Measurement;

State x = State::Zero();   // 初始状态
Matrix3d P = Matrix3d::Identity();  // 初始协方差矩阵

Matrix3d Ad = Matrix3d::Identity();
Matrix<double, 3, 2> Bd;
Matrix3d Qd = (Matrix3d() << Q_X, 0, 0, 0, Q_Y, 0, 0, 0, Q_THETA).finished();
Matrix<double, 2, 3> H = (Matrix<double, 2, 3>() << 1, 0, 0, 0, 1, 0).finished();
Matrix2d Rm = (Matrix2d() << R_X, 0, 0, R_Y).finished();

void update_matrices(double v, double theta) {
    Ad << 1, 0, -v * sin(theta) * DT,
          0, 1, v * cos(theta) * DT,
          0, 0, 1;
    Bd << cos(theta) * DT, 0,
          sin(theta) * DT, 0,
          0, DT;
}

void kalman_filter(const Measurement& z, double v, double omega) {
    update_matrices(v, x(2));

    // 预测步骤
    State u;
    u << v, omega;
    State x_pred = Ad * x + Bd * u;
    Matrix3d P_pred = Ad * P * Ad.transpose() + Qd;

    // 更新步骤
    Matrix<double, 3, 2> K = P_pred * H.transpose() * (H * P_pred * H.transpose() + Rm).inverse();
    x = x_pred + K * (z - H * x_pred);
    P = (Matrix3d::Identity() - K * H) * P_pred;
}

int main() {
    const int N = 100;
    Measurement z;
    double v, omega;

    vector<State> true_state(N);
    vector<State> est_state(N);
    vector<Measurement> measurement_data(N);
    vector<Vector2d> control_input(N);

    // 生成模拟数据
    for (int i = 0; i < N; ++i) {
        true_state[i] = State(i * DT, 0.5 * i * DT, 0.1 * i * DT);  // 模拟真实轨迹
        measurement_data[i] = true_state[i].head<2>() + Rm * Measurement::Random(); // 加入测量噪声
        control_input[i] = Vector2d(0.5, 0.1);  // 控制输入
    }

    // 卡尔曼滤波过程
    for (int k = 0; k < N; ++k) {
        kalman_filter(measurement_data[k], control_input[k][0], control_input[k][1]);
        est_state[k] = x;
    }

    // 计算误差并可视化
    vector<double> time(N), x_err(N), y_err(N), theta_err(N);
    for (int i = 0; i < N; ++i) {
        Vector3d error = true_state[i] - est_state[i];
        time[i] = i * DT;
        x_err[i] = error(0);
        y_err[i] = error(1);
        theta_err[i] = error(2);
    }

    plt::figure();
    plt::subplot(3, 1, 1);
    plt::plot(time, x_err, "r");
    plt::title("X 轴误差");

    plt::subplot(3, 1, 2);
    plt::plot(time, y_err, "g");
    plt::title("Y 轴误差");

    plt::subplot(3, 1, 3);
    plt::plot(time, theta_err, "b");
    plt::title("航向角误差");

    plt::show();

    return 0;
}

编写Cmake文件：

cmake_minimum_required(VERSION 3.10)
project(KalmanFilterVisualization)

set(CMAKE_CXX_STANDARD 11)

# 查找Eigen库
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS})

# 设置matplotlibcpp的路径
set(MATPLOTLIBCPP_INCLUDE_DIR "/path/to/matplotlibcpp/")

include_directories(${MATPLOTLIBCPP_INCLUDE_DIR})

# 生成可执行文件
add_executable(KalmanFilterVisualization main.cpp)

# 链接Eigen库
target_link_libraries(KalmanFilterVisualization Eigen3::Eigen)

最终测试结果如下图所示：

参考：

从理论到实践！轮趣科技发布卡尔曼滤波算法教程并提供算法验证平台 - 哔哩哔哩 (bilibili.com)

【卡尔曼滤波器】1_递归算法_Recursive Processing_哔哩哔哩_bilibili

从状态观测器角度理解卡尔曼滤波_观测器论文公式推导-CSDN博客