久久毛片,无码少妇一区二区三区,日韩国产欧美一区

/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "kalman.h"
#include "matrix.h"
/*==============================================================================
1.預估計
X(k|k-1) = F(k,k-1)*X(k-1|k-1) //控制量為0
2.計算預估計協方差矩陣
P(k|k-1) = F(k,k-1)*P(k-1|k-1)*F(k,k-1)'+Q(k)
Q(k) = U(k)×U(k)'
3.計算卡爾曼增益矩陣
Kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R(k))
R(k) = N(k)×N(k)'
4.更新估計
X(k|k) = X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))
5.計算更新后估計協防差矩陣
P(k|k) =（I-Kg(k)*H）*P(k|k-1)
6. 更新最優值
F(k,k-1): 狀態轉移矩陣
X(k|k-1): 根據k-1時刻的最優值估計k時刻的值
X(k-1|k-1): k-1時刻的最優值
P(k|k-1): X(k|k-1)對應的covariance
P(k-1|k-1): X(k-1|k-1)對應的covariance
Q(k): 系統過程的covariance
R(k): 測量過程的協方差
H(k): 觀測矩陣轉移矩陣
Z(k): k時刻的測量值
基本思路: 首先根據上一次(如果是第一次則根據預賦值計算)的數據計算出本次的估計值,
同理,根據上一次的數據計算出本次估計值的協方差; 接著,由本次估計值的協
方差計算出卡爾曼增益; 最后,根據估測值和測量值計算當前最優值及其協方差
==============================================================================*/
//================================================//
//== 最優值方差結構體 ==//
//================================================//
typedef struct _tCovariance
{
float PNowOpt[LENGTH];
float PPreOpt[LENGTH];
}tCovariance;
static tOptimal tOpt;
static tCovariance tCov;
//float Z[LENGTH] = {4000}; // 測量值(每次測量的數據需要存入該數組)
static float I[LENGTH] = {1}; // 單位矩陣
static float X[LENGTH] = {9.8}; // 當前狀態的預測值
static float P[LENGTH] = {0}; // 當前狀態的預測值的協方差
static float K[LENGTH] = {0}; // 卡爾曼增益
static float Temp3[LENGTH] = {0}; // 輔助變量
//============================================================================//
//== 卡爾曼濾波需要配置的變量 ==//
//============================================================================//
static float F[LENGTH] = {1}; // 狀態轉移矩陣即：堅信當前的狀態與上一次狀態的關系，如果堅信是一樣的，則狀態轉移矩陣就為單位矩陣
static float Q[LENGTH] = {0.0001f};//0.0001f // 系統過程的協方差協方差的定義：真實值與期望值之差的平方的期望值
static float R[LENGTH] = {2}; // 測量過程的協方差協方差的定義：真實值與期望值之差的平方的期望值
//如果你需要濾波結果更依賴于觀測量，那就調小R，增大Q；反之，調大R，調小Q，這樣估計值就取決于系統。
//如果R大Q小，就是說，狀態估計值比測量值要可靠，這時，所得出的結果就是更接近估計值；
//如果R小Q大，這時，計算出來的結果就會更接近測量值。
static float H[LENGTH] = {1}; // 觀測矩陣轉移矩陣測量值與狀態預測值之間的單位換算關系，即把預測值單位換算成測量值單位
static float Temp1[LENGTH] = {1}; // 輔助變量, 同時保存tOpt.XPreOpt[]的初始化值
static float Temp2[LENGTH] = {10000}; // 輔助變量, 同時保存tCov.PPreOpt[]的初始化值
void KalMan_PramInit(void)
{
unsigned char i;
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = Temp1[i]; //零值初始化
}
for (i=0; i<LENGTH; i++)
{
tCov.PPreOpt[i] = Temp2[i]; //零值初始化
}
}
//============================================================================//
//== 卡爾曼濾波 ==//
//============================================================================//
//==入口參數: 當前時刻的測量值 ==//
//==出口參數: 當前時刻的最優值 ==//
//==返回值: 當前時刻的最優值 ==//
//============================================================================//
float KalMan_Update(float *Z)
{
u8 i;
MatrixMul(F, tOpt.XPreOpt, X, ORDER, ORDER, ORDER); // 基于系統的上一狀態而預測現在狀態; X(k|k-1) = F(k,k-1)*X(k-1|k-1)
MatrixCal(F, tCov.PPreOpt, Temp1, ORDER);
MatrixAdd(Temp1, Q, P, ORDER, ORDER); // 預測數據的協方差矩陣; P(k|k-1) = F(k,k-1)*P(k-1|k-1)*F(k,k-1)'+Q
MatrixCal(H, P, Temp1, ORDER);
MatrixAdd(Temp1, R, Temp1, ORDER, ORDER);
Gauss_Jordan(Temp1, ORDER);
MatrixTrans(H, Temp2, ORDER, ORDER);
MatrixMul(P, Temp2, Temp3, ORDER, ORDER, ORDER);
MatrixMul(Temp1, Temp3, K, ORDER, ORDER, ORDER); // 計算卡爾曼增益; Kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
MatrixMul(H, X, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(Z, Temp1, Temp1, ORDER, ORDER);
MatrixMul(K, Temp1, Temp2, ORDER, ORDER, ORDER);
MatrixAdd(X, Temp2, tOpt.XNowOpt, ORDER, ORDER); // 根據估測值和測量值計算當前最優值; X(k|k) = X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))
MatrixMul(K, H, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(I, Temp1, Temp1, ORDER, ORDER);
MatrixMul(Temp1, P, tCov.PNowOpt, ORDER, ORDER, ORDER); // 計算更新后估計協防差矩陣; P(k|k) =（I-Kg(k)*H）*P(k|k-1)
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = tOpt.XNowOpt[i];
tCov.PPreOpt[i] = tCov.PNowOpt[i];
}
return tOpt.XNowOpt[0];
}

復制代碼

<li id="2oq22"></li>

<strike id="2oq22"></strike>

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费