卡尔曼滤波算法
程序员文章站
2022-07-12 09:37:42
...
c语言代码:
// fw.cpp : Defines the entry point for the console application.
//
#include "stdafx.h"
#include "stdio.h"
#include "stdlib.h"
#include "math.h"
double frand()
{
return 2 * ((rand() / (double)RAND_MAX) - 0.5);//随机噪声(-1~1之间):rand()/RAND_MAX为0~1之间
}
void main()
{
float x_last = 0;//初始估计值
float p_last = 0.02;//初始估计值方差
float Q = 0.018;//高斯噪声方差
float R = 0.542;//测量方差,反应测量的精度
float kg;//卡尔曼增益
float x_mid;
float x_now;
float p_mid;
float p_now;
float z_real = 0.56;//实际值(要测量的目标)
float z_measure;
float summerror_kalman = 0;//测量累积误差
float summerror_measure = 0;//估计值累积误差
int i;
x_last = z_real + frand()*0.03;
x_mid = x_last;
for (i = 0; i < 100;i++)
{
x_mid = x_last;//上一个卡尔曼估计值
p_mid = p_last + Q;//上一个估计值方差加上噪声方差,即实际方差
kg = p_mid / (p_mid + R);//新的卡尔曼增益系数,新估计值方差与新估计值方差加测量方差的比值。如果测量方差很大,显然卡尔曼系数变小,计算新估计值时,分配给测量值的权重就小一点
z_measure = z_real + frand()*0.03;//新的测量值,系统随机生成
x_now = x_mid + kg*(z_measure - x_mid);//即x[i]=(1-kg)*x[i-1]+kg*z[i],由测量值和上一次估计值共同决定这次估计值,信谁多一点由卡尔曼增益系数决定,卡尔曼增益系数又由每一轮的新方差决定
p_now = (1 - kg)*p_mid;//新的估计方差,原方差的卡尔曼信任度算得
printf("真实值:%6.3f\n", z_real);
printf("测量值:%6.3f [差值(绝对值):%.3f]\n", z_measure, fabs(z_real - z_measure));
printf("卡尔曼值: %6.3f [差值(绝对值):%.3f]\n", x_now, fabs(z_real - x_now));
printf("\n\n");
summerror_kalman += fabs(z_real - x_now);//卡尔曼误差累积
summerror_measure += fabs(z_real - z_measure);//测量误差累积
p_last = p_now;
x_last = x_now;
}
printf("总体测量误差累积 :%f\n", summerror_measure);
printf("总体卡尔曼滤波误差累积:%f\n", summerror_kalman);
printf("卡尔曼误差所占比例(越大越好咯):%d%%\n", 100 - (int)((summerror_kalman / summerror_measure) * 100));
getchar();
}
上一篇: 剑指offer:顺时针打印矩阵
下一篇: 卡尔曼滤波算法实例
推荐阅读
-
PHP实现克鲁斯卡尔算法实例解析,克鲁斯卡尔算法实例_PHP教程
-
PHP实现克鲁斯卡尔算法实例解析_PHP
-
迪菲-赫尔曼密钥交换(Diffie–Hellman)算法原理和PHP实现版
-
python点云地面点滤波(Progressive Morphological Filter)算法介绍(PCL库)
-
笛卡尔积算法的Java实现 算法java笛卡尔积
-
迪菲-赫尔曼密钥交换(Diffie–Hellman)算法原理和PHP实现版_php实例
-
Python2.7基于笛卡尔积算法实现N个数组的排列组合运算示例
-
.net C# 实现任意List的笛卡尔乘积算法代码
-
Java语言基于无向有权图实现克鲁斯卡尔算法代码示例
-
迪菲-赫尔曼密钥交换(Diffie–Hellman)算法原理和PHP实现版