在上一篇文章介绍并推导了卡尔曼滤波的基本原理之后，本文主要通过几个实例来讲解卡尔曼滤波的具体应用。
一.基于卡尔曼滤波的数据融合示例
假设使用一把最小刻度为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 是一款集成了三轴陀螺仪和三轴加速度计的传感器，广泛应用于姿态检测、运动跟踪和稳定控制等领域。具有如下主要特点：
- 三轴加速度计：可以测量设备在 X、Y、Z 轴上的线性加速度。
- 三轴陀螺仪：可以测量设备在 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)
扫一扫
417
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
