Kalman filter test harness with mimic C# code converted from Arduino code originally writen by Kristian Lauszus, TKJ Electronics. The filter inputs in the test harness are driven from the sliders but could easily be fed from a real sensor. Another day perhaps? The whole project is here Kalman Test
The C# class
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). Contact information ------------------- Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com */ using System; using System.Collections.Generic; using System.Linq; using System.Text; namespace KalmanTest { public class Kalman { /* variables */ double Q_angle; // Process noise variance for the accelerometer double Q_bias; // Process noise variance for the gyro bias double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise double angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrix double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate double[,] P = new double[2,2]; // Error covariance matrix - This is a 2x2 matrix double[] K = new double[2]; // Kalman gain - This is a 2x1 matrix double y; // Angle difference - 1x1 matrix double S; // Estimate error - 1x1 matrix
public Kalman() { /* We will set the varibles like so, these can also be tuned by the user */ Reset(); // Reset bias // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), //the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical SetDefaults(); } // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds public double getAngle(double newAngle, double newRate, double dt) { // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 // Modified by Kristian Lauszus // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it // Discrete Kalman filter time update equations - Time Update ("Predict") // Update xhat - Project the state ahead /* Step 1 */ rate = newRate - bias; angle += dt * rate; // Update estimation error covariance - Project the error covariance ahead /* Step 2 */ P[0,0] += dt * (dt*P[1,1] - P[0,1] - P[1,0] + Q_angle); P[0,1] -= dt * P[1,1]; P[1,0] -= dt * P[1,1]; P[1,1] += Q_bias * dt; // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") // Calculate Kalman gain - Compute the Kalman gain /* Step 4 */ S = P[0,0] + R_measure; /* Step 5 */ K[0] = P[0,0] / S; K[1] = P[1,0] / S; // Calculate angle and bias - Update estimate with measurement zk (newAngle) /* Step 3 */ y = newAngle - angle; /* Step 6 */ angle += K[0] * y; bias += K[1] * y; // Calculate estimation error covariance - Update the error covariance /* Step 7 */ P[0,0] -= K[0] * P[0,0]; P[0,1] -= K[0] * P[0,1]; P[1,0] -= K[1] * P[0,0]; P[1,1] -= K[1] * P[0,1]; return angle; } public void setAngle(double newAngle) { angle = newAngle; } // Used to set angle, this should be set as the starting angle public double getRate() { return rate; } // Return the unbiased rate /* These are used to tune the Kalman filter */ public void setQangle(double newQ_angle) { Q_angle = newQ_angle; } public double getQangle() { return Q_angle; } public void setQbias(double newQ_bias) { Q_bias = newQ_bias; } public double getQbias() { return Q_bias; } public void setRmeasure(double newR_measure) { R_measure = newR_measure; } public double getRmeasure() { return R_measure; }
public void Reset() { bias = 0; // Reset bias P[0, 0] = 0; // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical P[0, 1] = 0; P[1, 0] = 0; P[1, 1] = 0; } public void SetDefaults() { /* We will set the varibles like so, these can also be tuned by the user */ Q_angle = 0.001; Q_bias = 0.003; R_measure = 0.03; } }//End of class }
[nickatredbox] keeps up to date with the improvements of his project [yellow plane]. As you can find on this blog, the project is evolving week by week. Let’s see what’s today submission
1200 mm Wing space
280 mm cord
14% Clark Y
Target AUW 1300 Grams
Missing battery and camera box have a design which should weigh 140 grams empty.
The assembly shown below weighs 684 Grams no motor or electronics.
Electronics shown weigh 110 grams ESC Arduino board, Xbee, antenna and Gyro board
Motor & prop another 120 Gram
Here you have a [video] and there you can follow the project on the [website]
Revised Yellow FPV Plane with gyro stability system added, Worked best with analogue inputs from Murata piezo gyro sensors of a dead KK board filtered taking a 10 point average. Yaw compensation is currently not used as there is no rudder presently.
Gyro stability code in action
The RX board using the Arduino Nano weighs 12 grams
#include char buf[255] = {0, }; String str = ""; char *p; Servo servo[7]; // create servo object to control a servo int val = 0; // variable to read the value from the analog pin int AnInWOZ[MAX_CHAN] = {0, }; int AnIn[MAX_CHAN] = {0, }; int AnInBuf[MAX_CHAN] = {0, }; //Get the target values from the TX at rest int PitchTargWOZ = 9999; int RollTargWOZ = 9999; int YawTargWOZ = 9999; String inputString = ""; // a string to hold incoming data int Sample = 0; int TxTemp[MAX_CHAN + 1] = {0, }; int TxVal[MAX_CHAN + 1] = {0, }; int NavVal[MAX_NAV_VALS] = {0, }; int SettingVal[MAX_SETTINGS] = {0, }; int rssiDur = 0; int DigBits = 0; int ComState = 0; long PacketCount = 0; long NoPacketCount = 0; //Digital inputs TX code helper //TxVal[8] |= (digitalRead(5) << 0);//joy 2 push //TxVal[8] |= (digitalRead(6) << 1);//pb //TxVal[8] |= (digitalRead(7) << 2);//slide //TxVal[8] |= (digitalRead(8) << 3);//toggle void setup() { // initialize serial: Serial.begin(38400); // reserve 200 bytes for the inputString: inputString.reserve(MAX_IN_STR); pinMode(2,INPUT);//rssi digitalWrite(2, HIGH); servo[0].attach(3); // attaches the servo on pin 3 to the servo object servo[1].attach(5); // attaches the servo on pin 5 to the servo object servo[2].attach(6); // attaches the servo on pin 6 to the servo object servo[3].attach(9); // attaches the servo on pin 9 to the servo object servo[4].attach(10); // attaches the servo on pin 10 to the servo object servo[5].attach(11); // attaches the servo on pin 11 to the servo object NullServos(); //Get all the analogue signals //Do a wind off zero for(int i = 0;i < 8;i++) AnInWOZ[i] = 0; for(int i = 0;i < MAX_SAMPLE;i++){ AnIn[0] += analogRead(A0); AnIn[1] += analogRead(A1); AnIn[2] += analogRead(A2); AnIn[3] += analogRead(A3); AnIn[4] += analogRead(A4); AnIn[5] += analogRead(A5); AnIn[6] += analogRead(A6); AnIn[7] += analogRead(A7); delayMicroseconds(10); } for(int i = 0;i < 8;i++) AnInWOZ[i] = (AnIn[i] / MAX_SAMPLE); //Prime the WOZ values PitchTargWOZ = 9999; RollTargWOZ = 9999; YawTargWOZ = 9999; } void loop(){ //*Get all the analogue signals for(int i = 0;i < 8;i++) AnInBuf[i] = 0; for(int i = 0;i < MAX_SAMPLE;i++){ AnInBuf[0] += analogRead(A0); AnInBuf[1] += analogRead(A1); AnInBuf[2] += analogRead(A2); AnInBuf[3] += analogRead(A3); AnInBuf[4] += analogRead(A4); AnInBuf[5] += analogRead(A5); AnInBuf[6] += analogRead(A6); AnInBuf[7] += analogRead(A7); delayMicroseconds(10); } for(int i = 0;i < 8;i++) AnIn[i]= (AnInBuf[i] / MAX_SAMPLE); //Capture the Xbee comms int CharCount = 0; while ((Serial.available()) && ((++CharCount) < MAX_IN_STR) ) { // get the new byte: char inChar = (char)Serial.read(); //Determine packet type if ( (inChar == 'C') && (ComState == 0) ) ComState = 1; else if ( (inChar == 'N') && (ComState == 0) ) ComState = 2; else if ( (inChar == 'S') && (ComState == 0) ) ComState = 3; // add it to the inputString: if(ComState > 0) inputString += inChar; if(inputString.length() >= MAX_IN_STR) break; //Detect end of packet if ( (inChar == '\n') && (ComState > 0) ) { //Serial.println(inputString); //Count packets PacketCount++; int NumChan = ExtractPacket(); //Tramsmitter if( ComState == 1) { for(int i = 0 ;i < NumChan;i++) TxVal[i] = TxTemp[i]; DoTelemetery(); UpdateServos(); }//Navigator else if( ComState == 2) { for(int i = 0 ;i < NumChan;i++) NavVal[i] = TxTemp[i]; }//Settings else if( ComState == 3) { for(int i = 0 ;i < NumChan;i++) SettingVal[i] = TxTemp[i]; }//End of if( ComState == 0) ComState = 0; NoPacketCount = 0; break; } } //Count missing packets if((++NoPacketCount) > 50) { NullServos(); SoftReset(); } //delayMicroseconds(1000); } int ExtractPacket() { int Lchk = 0; int channel = 0; //initialise the channel count p = &inputString[0]; while ((str = strtok_r(p, ",", &p)) != NULL) // delimiter is the comma { TxTemp[channel] = str.toInt(); //use the channel as an index to add each value to the array Lchk += TxTemp[channel]; channel++; //increment the channel if(channel > MAX_CHAN) break; } p = NULL; inputString = "";
//Process in comming data if(channel > 2) { //Remove the remote chk from the total Lchk -= TxTemp[channel-2]; //Checksum if((Lchk - TxTemp[channel-2]) == 0) return channel; } return -1; } void DoTelemetery() { //Send back a telemetery packet if((PacketCount % 5) == 0) { rssiDur = pulseIn(5, LOW, 200); int PacketType = 12; //sprintf(buf, "T%02X,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n", PacketType, rssiDur, PacketCount, NoPacketCount, TxVal[1], TxVal[2], TxVal[3], TxVal[4], TxVal[5], TxVal[6], AnIn[0], AnIn[1], AnIn[2], AnIn[3], AnIn[4], AnIn[5], AnIn[6], AnIn[7], DigBits); //sprintf(buf, "T %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n", PacketType, rssiDur, PacketCount, NoPacketCount, AnIn[0], AnIn[1], AnIn[2], AnIn[3], AnIn[4], AnIn[5], AnIn[6], AnIn[7], DigBits, TxVal[9]); sprintf(buf, "T%02X%02X%04X%02X%03X%03X%03X%03X%03X%03X%03X%03X%03X%02X%02X\n", PacketType, rssiDur, PacketCount, NoPacketCount, AnIn[0], AnIn[1], AnIn[2], AnIn[3], AnIn[4], AnIn[5], AnIn[6], AnIn[7], DigBits, TxVal[9]); Serial.write(buf); if(digitalRead(13) == false) digitalWrite(13, HIGH); // set the LED on else digitalWrite(13, LOW); // set the LED off } } void UpdateServos() { //Digital inputs TX code helper //TxVal[8] |= (digitalRead(5) << 0);//joy 2 push //TxVal[8] |= (digitalRead(6) << 1);//pb //TxVal[8] |= (digitalRead(7) << 2);//slide //TxVal[8] |= (digitalRead(8) << 3);//toggle //Throttle TxVal[1] //Rotary pot TxVal[2] //Joy 1 X TxVal[3] //Joy 1 Y TxVal[4] //Joy 2 X TxVal[5] //Joy 2 Y TxVal[6] //rssi TxVal[7] //digital TxVal[8] //micros() TxVal[9] //Use the pot as the gain for all channels for now float GainPot = (float)(TxVal[2]) * 0.001f; //Get the target values from the TX int PitchTarg = (TxVal[3] / 10); int RollTarg = (TxVal[4] / 10); int YawTarg = (TxVal[6] / 10); //Prime the Target WOZ values if(PitchTargWOZ == 9999) PitchTargWOZ = PitchTarg; if(RollTargWOZ == 9999) RollTargWOZ = RollTarg; if(YawTargWOZ == 9999) YawTargWOZ = YawTarg; //Get the Centered target values float PitchTargCentred = (float)(PitchTarg - PitchTargWOZ); float RollTargCentred = (float)(RollTarg - RollTargWOZ); float YawTargCentred = (float)(YawTarg - YawTargWOZ); //Calculate gains float PitchGain = GainPot * 1.0f; float RollGain = GainPot * 1.0f; float YawGain = GainPot * 1.0f; //Get Gyro values float PitchGyro = (float)(AnIn[2] - AnInWOZ[2]); float RollGyro = (float)(AnIn[1] - AnInWOZ[1]); float YawGyro = (float)(AnIn[0] - AnInWOZ[0]); //Calc P error float PitchError = (float)PitchTargCentred + PitchGyro; float RollError = (float)RollTargCentred + RollGyro; float YawError = (float)YawTargCentred + YawGyro; //Apply gains int PitchTrim = (int)(PitchError * PitchGain); int RollTrim = (int)(RollError * RollGain); int YawTrim = (int)(YawError * YawGain); //Constaring trim authority PitchTrim = constrain(PitchTrim, -30, 30); RollTrim = constrain(RollTrim, -30, 30); YawTrim = constrain(YawTrim, -30, 30); //Dump the trim value if((TxVal[9] & 0x4) == 0) { PitchTrim = 0; RollTrim = 0; YawTrim = 0; } //Calc flap anglke int Flaps = 0; //Apply flaps if((TxVal[9] & 0x8) != 0) Flaps = 25; //Throttle val = TxVal[1] / 10; val = map(val, 1, 179, 30, 179); val = constrain(val, 1, 165); // scale it to use it with the servo (value between 0 and 180) servo[0].write(val); // sets the servo position according to the scaled value //Elevator Joy 1 Y TxVal[4] val = PitchTarg + PitchTrim; val = constrain(val, 15, 165); val = map(val, 0, 179, 135, 45); // scale it to use it with the servo (value between 0 and 180) servo[1].write(val); // sets the servo position according to the scaled value //Left Flaperon val = RollTarg + Flaps + RollTrim; val = constrain(val, 15, 165); val = map(val, 0, 179, 165, 15); // scale it to use it with the servo (value between 0 and 180) servo[2].write(val); // sets the servo position according to the scaled value //Right Flaperon val = RollTarg - Flaps + RollTrim; val = constrain(val, 15, 165); val = map(val, 0, 179, 165, 15); // scale it to use it with the servo (value between 0 and 180) servo[3].write(val); // sets the servo position according to the scaled value //Joy 2 x nose Wheel / rudder val = (TxVal[6] / 10); val = map(val, 0, 179, 55, 125); servo[4].write(val); // sets the servo position according to the scaled value //Joy 2 Y val = TxVal[5] / 10; val = constrain(val, 15, 165); // scale it to use it with the servo (value between 0 and 180) servo[5].write(val); // sets the servo position according to the scaled value } void NullServos() { //Throttle TxVal[1] //Rotary pot TxVal[2] //Joy 1 X TxVal[3] //Joy 1 Y TxVal[4] //Joy 2 X TxVal[5] //Joy 2 Y TxVal[6] //rssi TxVal[7] //digital TxVal[8] //micros() TxVal[9] //Throttle val = 0; val = map(val, 1, 179, 30, 179); val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180) servo[0].write(val); // sets the servo position according to the scaled value //Elevator Joy 1 Y TxVal[4] val = 90; val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180) servo[1].write(val); // sets the servo position according to the scaled value //Left Flaperon val = 90; val = map(val, 0, 179, 1, 179); // scale it to use it with the servo (value between 0 and 180) servo[2].write(val); // sets the servo position according to the scaled value //Right Flaperon val = 90; val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180) servo[3].write(val); // sets the servo position according to the scaled value //Joy 2 X val = 90; val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180) servo[4].write(val); // sets the servo position according to the scaled value //Joy 2 Y val = 90; val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180) servo[5].write(val); // sets the servo position according to the scaled value } void SoftReset() // Restarts program from beginning but does not reset the peripherals and registers { //Prime the WOZ values PitchTargWOZ = 9999; RollTargWOZ = 9999; YawTargWOZ = 9999; NoPacketCount = 0; asm volatile (" jmp 0"); }
About
Planet Arduino is, or at the moment is wishing to become, an aggregation of public weblogs from around the world written by people who develop, play, think on Arduino platform and his son. The opinions expressed in those weblogs and hence this aggregation are those of the original authors. Entries on this page are owned by their authors. We do not edit, endorse or vouch for the contents of individual posts. For more information about Arduino please visit www.arduino.cc
You are currently browsing the archives for the stability category.